From 03901c8c0dc537aca9bfbdcd45dac65891c88b4b Mon Sep 17 00:00:00 2001 From: Teravus Ovares Date: Sun, 19 Apr 2009 08:12:10 +0000 Subject: * Rudimentary angular motor implementation for the LSL Vehicle API --- OpenSim/Region/Physics/OdePlugin/ODECharacter.cs | 6 +- .../Region/Physics/OdePlugin/ODEVehicleSettings.cs | 86 ++++++++++++++++++++-- 2 files changed, 84 insertions(+), 8 deletions(-) (limited to 'OpenSim/Region') diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs index dae19c3..ed1366c 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs @@ -52,8 +52,12 @@ namespace OpenSim.Region.Physics.OdePlugin StopCFM = 8, LoStop2 = 256, HiStop2 = 257, + Vel2 = 258, + FMax2 = 259, LoStop3 = 512, - HiStop3 = 513 + HiStop3 = 513, + Vel3 = 514, + FMax3 = 515 } public class OdeCharacter : PhysicsActor { diff --git a/OpenSim/Region/Physics/OdePlugin/ODEVehicleSettings.cs b/OpenSim/Region/Physics/OdePlugin/ODEVehicleSettings.cs index 04de9a7..81c41ad 100644 --- a/OpenSim/Region/Physics/OdePlugin/ODEVehicleSettings.cs +++ b/OpenSim/Region/Physics/OdePlugin/ODEVehicleSettings.cs @@ -62,6 +62,7 @@ namespace OpenSim.Region.Physics.OdePlugin private Quaternion m_referenceFrame = Quaternion.Identity; private Vector3 m_angularFrictionTimescale = Vector3.Zero; private Vector3 m_angularMotorDirection = Vector3.Zero; + private Vector3 m_angularMotorDirectionLASTSET = Vector3.Zero; private Vector3 m_linearFrictionTimescale = Vector3.Zero; private Vector3 m_linearMotorDirection = Vector3.Zero; private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; @@ -84,6 +85,7 @@ namespace OpenSim.Region.Physics.OdePlugin private float m_verticalAttractionEfficiency = 0; private float m_verticalAttractionTimescale = 0; private Vector3 m_lastLinearVelocityVector = Vector3.Zero; + private Vector3 m_lastAngularVelocityVector = Vector3.Zero; private VehicleFlag m_flags = (VehicleFlag) 0; private bool m_LinearMotorSetLastFrame = false; @@ -168,6 +170,7 @@ namespace OpenSim.Region.Physics.OdePlugin break; case Vehicle.ANGULAR_MOTOR_DIRECTION: m_angularMotorDirection = new Vector3(pValue, pValue, pValue); + m_angularMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue); break; case Vehicle.LINEAR_FRICTION_TIMESCALE: m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); @@ -193,7 +196,7 @@ namespace OpenSim.Region.Physics.OdePlugin break; case Vehicle.ANGULAR_MOTOR_DIRECTION: m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); - m_LinearMotorSetLastFrame = true; + m_angularMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z); break; case Vehicle.LINEAR_FRICTION_TIMESCALE: m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); @@ -269,10 +272,12 @@ namespace OpenSim.Region.Physics.OdePlugin d.JointAttach(m_lMotor1, Body, IntPtr.Zero); } - Vector3 dirNorm = m_lastLinearVelocityVector; - dirNorm.Normalize(); - //d.JointSetLMotorAxis(m_lMotor1, 0, 1, dirNorm.X, dirNorm.Y, dirNorm.Z); - //d.JointSetLMotorParam(m_lMotor1, (int)dParam.Vel, m_lastLinearVelocityVector.Length()); + if (m_aMotor == IntPtr.Zero) + { + m_aMotor = d.JointCreateAMotor(pParentScene.world, m_jointGroup); + d.JointSetAMotorNumAxes(m_aMotor, 3); + d.JointAttach(m_aMotor, Body, IntPtr.Zero); + } } } @@ -296,6 +301,7 @@ namespace OpenSim.Region.Physics.OdePlugin return; VerticalAttractor(pTimestep); LinearMotor(pTimestep); + AngularMotor(pTimestep); } private void SetDefaultsForType(Vehicle pType) @@ -490,7 +496,7 @@ namespace OpenSim.Region.Physics.OdePlugin //System.Console.WriteLine(m_linearMotorDirection + " " + m_lastLinearVelocityVector); - SetMotorProperties(); + SetLinearMotorProperties(); Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep); m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount; @@ -499,7 +505,7 @@ namespace OpenSim.Region.Physics.OdePlugin } - private void SetMotorProperties() + private void SetLinearMotorProperties() { Vector3 dirNorm = m_lastLinearVelocityVector; dirNorm.Normalize(); @@ -519,5 +525,71 @@ namespace OpenSim.Region.Physics.OdePlugin } } + + private void AngularMotor(float pTimestep) + { + if (!m_angularMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) + { + + Vector3 addAmount = m_angularMotorDirection / (m_angularMotorTimescale / pTimestep); + m_lastAngularVelocityVector += (addAmount * 10); + + // This will work temporarily, but we really need to compare speed on an axis + if (Math.Abs(m_lastAngularVelocityVector.X) > Math.Abs(m_angularMotorDirectionLASTSET.X)) + m_lastAngularVelocityVector.X = m_angularMotorDirectionLASTSET.X; + if (Math.Abs(m_lastAngularVelocityVector.Y) > Math.Abs(m_angularMotorDirectionLASTSET.Y)) + m_lastAngularVelocityVector.Y = m_angularMotorDirectionLASTSET.Y; + if (Math.Abs(m_lastAngularVelocityVector.Z) > Math.Abs(m_angularMotorDirectionLASTSET.Z)) + m_lastAngularVelocityVector.Z = m_angularMotorDirectionLASTSET.Z; + //Console.WriteLine("add: " + addAmount); + + Vector3 decayfraction = ((Vector3.One / (m_angularMotorDecayTimescale / pTimestep))); + //Console.WriteLine("decay: " + decayfraction); + + m_angularMotorDirection -= m_angularMotorDirection * decayfraction; + //Console.WriteLine("actual: " + m_linearMotorDirection); + } + + //System.Console.WriteLine(m_linearMotorDirection + " " + m_lastLinearVelocityVector); + + SetAngularMotorProperties(); + + Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep); + m_lastAngularVelocityVector -= m_lastAngularVelocityVector * decayamount; + + //m_linearMotorDirection *= decayamount; + + } + private void SetAngularMotorProperties() + { + + + + d.Mass objMass; + d.BodyGetMass(Body, out objMass); + d.Quaternion rot = d.BodyGetQuaternion(Body); + Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); + Vector3 axis0 = Vector3.UnitX; + Vector3 axis1 = Vector3.UnitY; + Vector3 axis2 = Vector3.UnitZ; + axis0 *= rotq; + axis1 *= rotq; + axis2 *= rotq; + + + if (m_aMotor != IntPtr.Zero) + { + d.JointSetAMotorAxis(m_aMotor, 0, 1, axis0.X, axis0.Y, axis0.Z); + d.JointSetAMotorAxis(m_aMotor, 1, 1, axis1.X, axis1.Y, axis1.Z); + d.JointSetAMotorAxis(m_aMotor, 2, 1, axis2.X, axis2.Y, axis2.Z); + d.JointSetAMotorParam(m_aMotor, (int)dParam.FMax, 30*objMass.mass); + d.JointSetAMotorParam(m_aMotor, (int)dParam.FMax2, 30*objMass.mass); + d.JointSetAMotorParam(m_aMotor, (int)dParam.FMax3, 30 * objMass.mass); + d.JointSetAMotorParam(m_aMotor, (int)dParam.Vel, m_lastAngularVelocityVector.X); + d.JointSetAMotorParam(m_aMotor, (int)dParam.Vel2, m_lastAngularVelocityVector.Y); + d.JointSetAMotorParam(m_aMotor, (int)dParam.Vel3, m_lastAngularVelocityVector.Z); + + } + } } } -- cgit v1.1