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(-)
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