diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
index f4aa231dc1..6ffcb9e145 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODECharacter.cs
@@ -686,6 +686,9 @@ namespace OpenSim.Region.Physics.OdePlugin
Body = d.BodyCreate(_parent_scene.world);
+ _zeroFlag = false;
+ m_pidControllerActive = true;
+
d.BodySetAutoDisableFlag(Body, false);
d.BodySetPosition(Body, npositionX, npositionY, npositionZ);
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
index 49766f8f9a..7c0bbef99c 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
@@ -98,10 +98,12 @@ namespace OpenSim.Region.Physics.OdePlugin
private Vector3 m_forceacc;
private Vector3 m_angularForceacc;
+ private float m_invTimeStep = 50.0f;
+ private float m_timeStep = .02f;
+
+
private Vector3 m_PIDTarget;
private float m_PIDTau;
- private float PID_D = 35f;
- private float PID_G = 25f;
private bool m_usePID;
// KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
@@ -632,7 +634,6 @@ namespace OpenSim.Region.Physics.OdePlugin
Vector3 pv = Vector3.Zero;
if (_zeroFlag)
return pv;
- m_lastUpdateSent = false;
if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f))
return pv;
@@ -686,12 +687,50 @@ namespace OpenSim.Region.Physics.OdePlugin
}
public override bool PIDActive { set { m_usePID = value; } }
- public override float PIDTau { set { m_PIDTau = value; } }
+ public override float PIDTau
+ {
+ set
+ {
+ if (value <= 0)
+ m_PIDTau = 0;
+ else
+ {
+ float mint = (0.05f > _parent_scene.ODE_STEPSIZE ? 0.05f : _parent_scene.ODE_STEPSIZE);
+ if (value < mint)
+ m_PIDTau = mint;
+ else
+ m_PIDTau = value;
+ }
+ }
+ }
- public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
+ public override float PIDHoverHeight
+ {
+ set
+ {
+ m_PIDHoverHeight = value;
+ if (value == 0)
+ m_useHoverPID = false;
+ }
+ }
public override bool PIDHoverActive { set { m_useHoverPID = value; } }
public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
- public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
+ public override float PIDHoverTau
+ {
+ set
+ {
+ if (value <= 0)
+ m_PIDHoverTau = 0;
+ else
+ {
+ float mint = (0.05f > _parent_scene.ODE_STEPSIZE ? 0.05f : _parent_scene.ODE_STEPSIZE);
+ if (value < mint)
+ m_PIDHoverTau = mint;
+ else
+ m_PIDHoverTau = value;
+ }
+ }
+ }
public override Quaternion APIDTarget { set { return; } }
@@ -912,8 +951,9 @@ namespace OpenSim.Region.Physics.OdePlugin
_position = pos;
givefakepos = 0;
- PID_D = parent_scene.bodyPIDD;
- PID_G = parent_scene.bodyPIDG;
+ m_timeStep = parent_scene.ODE_STEPSIZE;
+ m_invTimeStep = 1f / m_timeStep;
+
m_density = parent_scene.geomDefaultDensity;
// m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
@@ -2696,12 +2736,6 @@ namespace OpenSim.Region.Physics.OdePlugin
private void DoSelectedStatus(bool newval)
{
- if (m_isSelected == newval)
- {
- resetCollisionAccounting();
- return;
- }
-
m_isSelected = newval;
Stop();
@@ -2970,7 +3004,6 @@ namespace OpenSim.Region.Physics.OdePlugin
givefakeori--;
if (givefakeori < 0)
givefakeori = 0;
-
resetCollisionAccounting();
}
@@ -3125,9 +3158,10 @@ namespace OpenSim.Region.Physics.OdePlugin
d.BodyEnable(Body);
}
- private void changeAddForce(Vector3 force)
+
+ private void changeAddImpulse(Vector3 impulse)
{
- m_forceacc += force;
+ m_forceacc += impulse * m_invTimeStep;
if (!m_isSelected)
{
lock (this)
@@ -3146,9 +3180,10 @@ namespace OpenSim.Region.Physics.OdePlugin
}
}
- private void changeAddAngularForce(Vector3 aforce)
+ // actually angular impulse
+ private void changeAddAngularImpulse(Vector3 aimpulse)
{
- m_angularForceacc += aforce;
+ m_angularForceacc += aimpulse * m_invTimeStep;
if (!m_isSelected)
{
lock (this)
@@ -3271,319 +3306,7 @@ namespace OpenSim.Region.Physics.OdePlugin
if (!childPrim && m_isphysical && Body != IntPtr.Zero &&
!m_disabled && !m_isSelected && !m_building && !m_outbounds)
{
- if (d.BodyIsEnabled(Body))
- {
- float timestep = _parent_scene.ODE_STEPSIZE;
-
- // check outside region
- d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator
-
- if (lpos.Z < -100 || lpos.Z > 100000f)
- {
- m_outbounds = true;
-
- lpos.Z = Util.Clip(lpos.Z, -100f, 100000f);
- _acceleration.X = 0;
- _acceleration.Y = 0;
- _acceleration.Z = 0;
-
- _velocity.X = 0;
- _velocity.Y = 0;
- _velocity.Z = 0;
- m_rotationalVelocity.X = 0;
- m_rotationalVelocity.Y = 0;
- m_rotationalVelocity.Z = 0;
-
- d.BodySetLinearVel(Body, 0, 0, 0); // stop it
- d.BodySetAngularVel(Body, 0, 0, 0); // stop it
- d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere
- m_lastposition = _position;
- m_lastorientation = _orientation;
-
- base.RequestPhysicsterseUpdate();
-
- m_throttleUpdates = false;
- throttleCounter = 0;
- _zeroFlag = true;
-
- disableBodySoft(); // disable it and colisions
- base.RaiseOutOfBounds(_position);
- return;
- }
-
- if (lpos.X < 0f)
- {
- _position.X = Util.Clip(lpos.X, -2f, -0.1f);
- m_outbounds = true;
- }
- else if (lpos.X > _parent_scene.WorldExtents.X)
- {
- _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f);
- m_outbounds = true;
- }
- if (lpos.Y < 0f)
- {
- _position.Y = Util.Clip(lpos.Y, -2f, -0.1f);
- m_outbounds = true;
- }
- else if (lpos.Y > _parent_scene.WorldExtents.Y)
- {
- _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f);
- m_outbounds = true;
- }
-
- if (m_outbounds)
- {
- m_lastposition = _position;
- m_lastorientation = _orientation;
-
- d.Vector3 dtmp = d.BodyGetAngularVel(Body);
- m_rotationalVelocity.X = dtmp.X;
- m_rotationalVelocity.Y = dtmp.Y;
- m_rotationalVelocity.Z = dtmp.Z;
-
- dtmp = d.BodyGetLinearVel(Body);
- _velocity.X = dtmp.X;
- _velocity.Y = dtmp.Y;
- _velocity.Z = dtmp.Z;
-
- d.BodySetLinearVel(Body, 0, 0, 0); // stop it
- d.BodySetAngularVel(Body, 0, 0, 0);
- d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
- disableBodySoft(); // stop collisions
- base.RequestPhysicsterseUpdate();
- return;
- }
-
- if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
- {
- // 'VEHICLES' are dealt with in ODEDynamics.cs
- m_vehicle.Step();
- }
- else
- {
- float fx = 0;
- float fy = 0;
- float fz = 0;
-
- float m_mass = _mass;
-
- // fz = 0f;
- //m_log.Info(m_collisionFlags.ToString());
- if (m_usePID)
- {
-
- // If the PID Controller isn't active then we set our force
- // calculating base velocity to the current position
-
- if ((m_PIDTau < 1) && (m_PIDTau != 0))
- {
- //PID_G = PID_G / m_PIDTau;
- m_PIDTau = 1;
- }
-
- if ((PID_G - m_PIDTau) <= 0)
- {
- PID_G = m_PIDTau + 1;
- }
-
- d.Vector3 vel = d.BodyGetLinearVel(Body);
- d.Vector3 pos = d.BodyGetPosition(Body);
- _target_velocity =
- new Vector3(
- (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
- (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
- (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
- );
-
- // if velocity is zero, use position control; otherwise, velocity control
-
- if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
- {
- // keep track of where we stopped. No more slippin' & slidin'
-
- // We only want to deactivate the PID Controller if we think we want to have our surrogate
- // react to the physics scene by moving it's position.
- // Avatar to Avatar collisions
- // Prim to avatar collisions
-
- //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
- //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
- //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
- d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
- d.BodySetLinearVel(Body, 0, 0, 0);
- // d.BodyAddForce(Body, 0, 0, fz);
- return;
- }
- else
- {
- _zeroFlag = false;
-
- // We're flying and colliding with something
- fx = ((_target_velocity.X) - vel.X) * (PID_D);
- fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
-
- // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
-
- fz = ((_target_velocity.Z - vel.Z) * (PID_D));
- }
- } // end if (m_usePID)
-
- // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
- else if (m_useHoverPID)
- {
- //Console.WriteLine("Hover " + Name);
-
- // If we're using the PID controller, then we have no gravity
-
- // no lock; for now it's only called from within Simulate()
-
- // If the PID Controller isn't active then we set our force
- // calculating base velocity to the current position
-
- if ((m_PIDTau < 1))
- {
- PID_G = PID_G / m_PIDTau;
- }
-
- if ((PID_G - m_PIDTau) <= 0)
- {
- PID_G = m_PIDTau + 1;
- }
-
- // Where are we, and where are we headed?
- d.Vector3 pos = d.BodyGetPosition(Body);
- d.Vector3 vel = d.BodyGetLinearVel(Body);
-
- // Non-Vehicles have a limited set of Hover options.
- // determine what our target height really is based on HoverType
- switch (m_PIDHoverType)
- {
- case PIDHoverType.Ground:
- m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
-
- break;
- case PIDHoverType.GroundAndWater:
- m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
- m_waterHeight = _parent_scene.GetWaterLevel();
- if (m_groundHeight > m_waterHeight)
- {
- if (m_PIDHoverHeight > 0 || m_isVolumeDetect)
- m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
- else
- m_targetHoverHeight = m_groundHeight;
- }
- else
- {
- m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
- }
- break;
-
- } // end switch (m_PIDHoverType)
-
- // don't go underground unless volumedetector
-
- if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect)
- {
- fz = (m_targetHoverHeight - pos.Z) * (PID_G - m_PIDHoverTau) * timestep;
-
- // if velocity is zero, use position control; otherwise, velocity control
-
- if (Math.Abs(fz) < 0.1f)
- {
- // keep track of where we stopped. No more slippin' & slidin'
-
- // We only want to deactivate the PID Controller if we think we want to have our surrogate
- // react to the physics scene by moving it's position.
- // Avatar to Avatar collisions
- // Prim to avatar collisions
-
- d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
- d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
- // ? d.BodyAddForce(Body, 0, 0, fz);
- return;
- }
- else
- {
- _zeroFlag = false;
-
- // We're flying and colliding with something
- fz = ((fz - vel.Z) * (PID_D));
- }
- }
- }
- else
- {
- float b = (1.0f - m_buoyancy);
- fx = _parent_scene.gravityx * b;
- fy = _parent_scene.gravityy * b;
- fz = _parent_scene.gravityz * b;
- }
-
- fx *= m_mass;
- fy *= m_mass;
- fz *= m_mass;
-
- // constant force
- fx += m_force.X;
- fy += m_force.Y;
- fz += m_force.Z;
-
- fx += m_forceacc.X;
- fy += m_forceacc.Y;
- fz += m_forceacc.Z;
-
- m_forceacc = Vector3.Zero;
-
- //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
- if (fx != 0 || fy != 0 || fz != 0)
- {
- d.BodyAddForce(Body, fx, fy, fz);
- //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
- }
-
- Vector3 trq;
-
- trq = _torque;
- trq += m_angularForceacc;
- m_angularForceacc = Vector3.Zero;
- if (trq.X != 0 || trq.Y != 0 || trq.Z != 0)
- {
- d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z);
- }
- }
-
- // update our ideia of velocities and acelerations
- d.Quaternion ori;
- d.Vector3 dtmpu;
-
- _position.X = lpos.X;
- _position.Y = lpos.Y;
- _position.Z = lpos.Z;
-
- d.GeomCopyQuaternion(prim_geom, out ori);
- _orientation.X = ori.X;
- _orientation.Y = ori.Y;
- _orientation.Z = ori.Z;
- _orientation.W = ori.W;
-
- _acceleration = _velocity;
-
- dtmpu = d.BodyGetLinearVel(Body);
- _velocity.X = dtmpu.X;
- _velocity.Y = dtmpu.Y;
- _velocity.Z = dtmpu.Z;
-
- float invts = 1 / timestep;
- _acceleration = (_velocity - _acceleration) * invts;
-
- dtmpu = d.BodyGetAngularVel(Body);
- m_rotationalVelocity.X = dtmpu.X;
- m_rotationalVelocity.Y = dtmpu.Y;
- m_rotationalVelocity.Z = dtmpu.Z;
- }
-
- else // body disabled/sleeping
+ if (!d.BodyIsEnabled(Body))
{
// let vehicles sleep
if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
@@ -3594,8 +3317,236 @@ namespace OpenSim.Region.Physics.OdePlugin
bodydisablecontrol = 0;
d.BodyEnable(Body);
+ }
+
+ // check outside region
+ d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator
+
+ if (lpos.Z < -100 || lpos.Z > 100000f)
+ {
+ m_outbounds = true;
+
+ lpos.Z = Util.Clip(lpos.Z, -100f, 100000f);
+ _acceleration.X = 0;
+ _acceleration.Y = 0;
+ _acceleration.Z = 0;
+
+ _velocity.X = 0;
+ _velocity.Y = 0;
+ _velocity.Z = 0;
+ m_rotationalVelocity.X = 0;
+ m_rotationalVelocity.Y = 0;
+ m_rotationalVelocity.Z = 0;
+
+ d.BodySetLinearVel(Body, 0, 0, 0); // stop it
+ d.BodySetAngularVel(Body, 0, 0, 0); // stop it
+ d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere
+ m_lastposition = _position;
+ m_lastorientation = _orientation;
+
+ base.RequestPhysicsterseUpdate();
+
+ throttleCounter = 0;
+ _zeroFlag = true;
+
+ disableBodySoft(); // disable it and colisions
+ base.RaiseOutOfBounds(_position);
return;
}
+
+ if (lpos.X < 0f)
+ {
+ _position.X = Util.Clip(lpos.X, -2f, -0.1f);
+ m_outbounds = true;
+ }
+ else if (lpos.X > _parent_scene.WorldExtents.X)
+ {
+ _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f);
+ m_outbounds = true;
+ }
+ if (lpos.Y < 0f)
+ {
+ _position.Y = Util.Clip(lpos.Y, -2f, -0.1f);
+ m_outbounds = true;
+ }
+ else if (lpos.Y > _parent_scene.WorldExtents.Y)
+ {
+ _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f);
+ m_outbounds = true;
+ }
+
+ if (m_outbounds)
+ {
+ m_lastposition = _position;
+ m_lastorientation = _orientation;
+
+ d.Vector3 dtmp = d.BodyGetAngularVel(Body);
+ m_rotationalVelocity.X = dtmp.X;
+ m_rotationalVelocity.Y = dtmp.Y;
+ m_rotationalVelocity.Z = dtmp.Z;
+
+ dtmp = d.BodyGetLinearVel(Body);
+ _velocity.X = dtmp.X;
+ _velocity.Y = dtmp.Y;
+ _velocity.Z = dtmp.Z;
+
+ d.BodySetLinearVel(Body, 0, 0, 0); // stop it
+ d.BodySetAngularVel(Body, 0, 0, 0);
+ d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
+ disableBodySoft(); // stop collisions
+ base.RequestPhysicsterseUpdate();
+ return;
+ }
+
+ if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
+ {
+ // 'VEHICLES' are dealt with in ODEDynamics.cs
+ m_vehicle.Step();
+ return;
+ }
+
+ float fx = 0;
+ float fy = 0;
+ float fz = 0;
+
+ float m_mass = _mass;
+
+ if (m_usePID && m_PIDTau > 0)
+ {
+ // for now position error
+ _target_velocity =
+ new Vector3(
+ (m_PIDTarget.X - lpos.X),
+ (m_PIDTarget.Y - lpos.Y),
+ (m_PIDTarget.Z - lpos.Z)
+ );
+
+ if (_target_velocity.ApproxEquals(Vector3.Zero, 0.02f))
+ {
+ d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
+ d.BodySetLinearVel(Body, 0, 0, 0);
+ return;
+ }
+ else
+ {
+ _zeroFlag = false;
+
+ float tmp = 1 / m_PIDTau;
+ _target_velocity *= tmp;
+
+ // apply limits
+ tmp = _target_velocity.Length();
+ if (tmp > 50.0f)
+ {
+ tmp = 50 / tmp;
+ _target_velocity *= tmp;
+ }
+ else if (tmp < 0.05f)
+ {
+ tmp = 0.05f / tmp;
+ _target_velocity *= tmp;
+ }
+
+ d.Vector3 vel = d.BodyGetLinearVel(Body);
+ fx = (_target_velocity.X - vel.X) * m_invTimeStep;
+ fy = (_target_velocity.Y - vel.Y) * m_invTimeStep;
+ fz = (_target_velocity.Z - vel.Z) * m_invTimeStep;
+ }
+ } // end if (m_usePID)
+
+ // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
+ else if (m_useHoverPID && m_PIDHoverTau != 0 && m_PIDHoverHeight != 0)
+ {
+
+ // Non-Vehicles have a limited set of Hover options.
+ // determine what our target height really is based on HoverType
+
+ m_groundHeight = _parent_scene.GetTerrainHeightAtXY(lpos.X, lpos.Y);
+
+ switch (m_PIDHoverType)
+ {
+ case PIDHoverType.Ground:
+ m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
+ break;
+
+ case PIDHoverType.GroundAndWater:
+ m_waterHeight = _parent_scene.GetWaterLevel();
+ if (m_groundHeight > m_waterHeight)
+ m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
+ else
+ m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
+ break;
+ } // end switch (m_PIDHoverType)
+
+ // don't go underground unless volumedetector
+
+ if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect)
+ {
+ d.Vector3 vel = d.BodyGetLinearVel(Body);
+
+ fz = (m_targetHoverHeight - lpos.Z);
+
+ // if error is zero, use position control; otherwise, velocity control
+ if (Math.Abs(fz) < 0.01f)
+ {
+ d.BodySetPosition(Body, lpos.X, lpos.Y, m_targetHoverHeight);
+ d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
+ return;
+ }
+ else
+ {
+ _zeroFlag = false;
+ fz /= m_PIDHoverTau;
+
+ float tmp = Math.Abs(fz);
+ if (tmp > 50)
+ fz = 50 * Math.Sign(fz);
+ else if (tmp < 0.1)
+ fz = 0.1f * Math.Sign(fz);
+
+ fz = ((fz - vel.Z) * m_invTimeStep);
+ }
+ }
+ }
+ else
+ {
+ float b = (1.0f - m_buoyancy);
+ fx = _parent_scene.gravityx * b;
+ fy = _parent_scene.gravityy * b;
+ fz = _parent_scene.gravityz * b;
+ }
+
+ fx *= m_mass;
+ fy *= m_mass;
+ fz *= m_mass;
+
+ // constant force
+ fx += m_force.X;
+ fy += m_force.Y;
+ fz += m_force.Z;
+
+ fx += m_forceacc.X;
+ fy += m_forceacc.Y;
+ fz += m_forceacc.Z;
+
+ m_forceacc = Vector3.Zero;
+
+ //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
+ if (fx != 0 || fy != 0 || fz != 0)
+ {
+ d.BodyAddForce(Body, fx, fy, fz);
+ //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
+ }
+
+ Vector3 trq;
+
+ trq = _torque;
+ trq += m_angularForceacc;
+ m_angularForceacc = Vector3.Zero;
+ if (trq.X != 0 || trq.Y != 0 || trq.Z != 0)
+ {
+ d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z);
+ }
}
else
{ // is not physical, or is not a body or is selected
@@ -3606,92 +3557,113 @@ namespace OpenSim.Region.Physics.OdePlugin
}
}
-
- public void UpdatePositionAndVelocity(float simulatedtime)
+ public void UpdatePositionAndVelocity()
{
- // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
- if (_parent == null && !m_disabled && !m_building && !m_outbounds)
+ if (_parent == null && !m_disabled && !m_building && !m_outbounds && Body != IntPtr.Zero)
{
- if (Body != IntPtr.Zero)
+ if (d.BodyIsEnabled(Body) || !_zeroFlag)
{
bool lastZeroFlag = _zeroFlag;
- if ((Math.Abs(m_lastposition.X - _position.X) < 0.01)
- && (Math.Abs(m_lastposition.Y - _position.Y) < 0.01)
- && (Math.Abs(m_lastposition.Z - _position.Z) < 0.01)
- && (Math.Abs(m_lastorientation.X - _orientation.X) < 0.0001)
- && (Math.Abs(m_lastorientation.Y - _orientation.Y) < 0.0001)
- && (Math.Abs(m_lastorientation.Z - _orientation.Z) < 0.0001)
+ d.Vector3 lpos;
+ d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator
+
+ d.Quaternion ori;
+ d.GeomCopyQuaternion(prim_geom, out ori);
+
+ // decide if moving
+ // use positions since this are integrated quantities
+ // tolerance values depende a lot on simulation noise...
+ // use simple math.abs since we dont need to be exact
+
+ if (
+ (Math.Abs(_position.X - lpos.X) < 0.001f)
+ && (Math.Abs(_position.Y - lpos.Y) < 0.001f)
+ && (Math.Abs(_position.Z - lpos.Z) < 0.001f)
+ && (Math.Abs(_orientation.X - ori.X) < 0.0001f)
+ && (Math.Abs(_orientation.Y - ori.Y) < 0.0001f)
+ && (Math.Abs(_orientation.Z - ori.Z) < 0.0001f) // ignore W
)
{
_zeroFlag = true;
- m_throttleUpdates = false;
}
else
- {
_zeroFlag = false;
- m_lastUpdateSent = false;
+
+ // update velocities and aceleration
+ if (!(_zeroFlag && lastZeroFlag))
+ {
+ d.Vector3 vel = d.BodyGetLinearVel(Body);
+
+ _acceleration = _velocity;
+
+ if ((Math.Abs(vel.X) < 0.001f) &&
+ (Math.Abs(vel.Y) < 0.001f) &&
+ (Math.Abs(vel.Z) < 0.001f))
+ {
+ _velocity = Vector3.Zero;
+ float t = -m_invTimeStep;
+ _acceleration = _acceleration * t;
+ }
+ else
+ {
+ _velocity.X = vel.X;
+ _velocity.Y = vel.Y;
+ _velocity.Z = vel.Z;
+ _acceleration = (_velocity - _acceleration) * m_invTimeStep;
+ }
+
+ if ((Math.Abs(_acceleration.X) < 0.01f) &&
+ (Math.Abs(_acceleration.Y) < 0.01f) &&
+ (Math.Abs(_acceleration.Z) < 0.01f))
+ {
+ _acceleration = Vector3.Zero;
+ }
+
+ if ((Math.Abs(_orientation.X - ori.X) < 0.0001) &&
+ (Math.Abs(_orientation.Y - ori.Y) < 0.0001) &&
+ (Math.Abs(_orientation.Z - ori.Z) < 0.0001)
+ )
+ {
+ m_rotationalVelocity = Vector3.Zero;
+ }
+ else
+ {
+ vel = d.BodyGetAngularVel(Body);
+ m_rotationalVelocity.X = vel.X;
+ m_rotationalVelocity.Y = vel.Y;
+ m_rotationalVelocity.Z = vel.Z;
+ }
}
if (_zeroFlag)
{
- m_lastposition = _position;
- m_lastorientation = _orientation;
-
- _velocity = Vector3.Zero;
- _acceleration = Vector3.Zero;
- m_rotationalVelocity = Vector3.Zero;
+ if (lastZeroFlag)
+ {
+ _velocity = Vector3.Zero;
+ _acceleration = Vector3.Zero;
+ m_rotationalVelocity = Vector3.Zero;
+ }
if (!m_lastUpdateSent)
{
- m_throttleUpdates = false;
- throttleCounter = 0;
-
base.RequestPhysicsterseUpdate();
-
- m_lastUpdateSent = true;
+ if (lastZeroFlag)
+ m_lastUpdateSent = true;
}
+ return;
}
- else
- {
- if (lastZeroFlag != _zeroFlag)
- {
- base.RequestPhysicsterseUpdate();
- }
- m_lastUpdateSent = false;
- if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
- {
- m_lastposition = _position;
- m_lastorientation = _orientation;
- m_lastVelocity = _velocity;
- base.RequestPhysicsterseUpdate();
- }
- else
- {
- throttleCounter++;
- }
- }
- }
- else if (!m_lastUpdateSent || !_zeroFlag)
- {
- // Not a body.. so Make sure the client isn't interpolating
- _velocity = Vector3.Zero;
- _acceleration = Vector3.Zero;
- m_rotationalVelocity = Vector3.Zero;
- m_lastVelocity = Vector3.Zero;
+ _position.X = lpos.X;
+ _position.Y = lpos.Y;
+ _position.Z = lpos.Z;
- _zeroFlag = true;
-
- if (!m_lastUpdateSent)
- {
- m_throttleUpdates = false;
- throttleCounter = 0;
-
- base.RequestPhysicsterseUpdate();
-
- m_lastUpdateSent = true;
- }
+ _orientation.X = ori.X;
+ _orientation.Y = ori.Y;
+ _orientation.Z = ori.Z;
+ _orientation.W = ori.W;
+ base.RequestPhysicsterseUpdate();
+ m_lastUpdateSent = false;
}
}
}
@@ -3849,11 +3821,11 @@ namespace OpenSim.Region.Physics.OdePlugin
break;
case changes.AddForce:
- changeAddForce((Vector3)arg);
+ changeAddImpulse((Vector3)arg);
break;
case changes.AddAngForce:
- changeAddAngularForce((Vector3)arg);
+ changeAddAngularImpulse((Vector3)arg);
break;
case changes.AngLock:
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
index 72ac605d15..1d9fa93c5e 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
@@ -521,7 +521,7 @@ namespace OpenSim.Region.Physics.OdePlugin
d.WorldSetAngularDamping(world, 0.001f);
d.WorldSetAngularDampingThreshold(world, 0f);
d.WorldSetLinearDampingThreshold(world, 0f);
- d.WorldSetMaxAngularSpeed(world, 256f);
+ d.WorldSetMaxAngularSpeed(world, 100f);
d.WorldSetCFM(world,1e-6f); // a bit harder than default
//d.WorldSetCFM(world, 1e-4f); // a bit harder than default
@@ -1685,17 +1685,6 @@ namespace OpenSim.Region.Physics.OdePlugin
///
public override float Simulate(float timeStep)
{
- int statstart;
- int statchanges = 0;
- int statchmove = 0;
- int statactmove = 0;
- int statray = 0;
- int statcol = 0;
- int statstep = 0;
- int statmovchar = 0;
- int statmovprim;
- int totjcontact = 0;
-
// acumulate time so we can reduce error
step_time += timeStep;
@@ -1738,8 +1727,6 @@ namespace OpenSim.Region.Physics.OdePlugin
{
try
{
- statstart = Util.EnvironmentTickCount();
-
// clear pointer/counter to contacts to pass into joints
m_global_contactcount = 0;
@@ -1778,17 +1765,39 @@ namespace OpenSim.Region.Physics.OdePlugin
}
- statchanges += Util.EnvironmentTickCountSubtract(statstart);
+ // Move characters
+ lock (_characters)
+ {
+ List defects = new List();
+ foreach (OdeCharacter actor in _characters)
+ {
+ if (actor != null)
+ actor.Move(ODE_STEPSIZE, defects);
+ }
+ if (defects.Count != 0)
+ {
+ foreach (OdeCharacter defect in defects)
+ {
+ RemoveCharacter(defect);
+ }
+ }
+ }
+
+ // Move other active objects
+ lock (_activegroups)
+ {
+ foreach (OdePrim aprim in _activegroups)
+ {
+ aprim.Move();
+ }
+ }
- statactmove += Util.EnvironmentTickCountSubtract(statstart);
//if ((framecount % m_randomizeWater) == 0)
// randomizeWater(waterlevel);
m_rayCastManager.ProcessQueuedRequests();
- statray += Util.EnvironmentTickCountSubtract(statstart);
collision_optimized();
- statcol += Util.EnvironmentTickCountSubtract(statstart);
lock (_collisionEventPrim)
{
@@ -1813,38 +1822,39 @@ namespace OpenSim.Region.Physics.OdePlugin
}
}
+ // do a ode simulation step
d.WorldQuickStep(world, ODE_STEPSIZE);
- statstep += Util.EnvironmentTickCountSubtract(statstart);
+ d.JointGroupEmpty(contactgroup);
- // Move characters
- lock (_characters)
+ // update managed ideia of physical data and do updates to core
+ /*
+ lock (_characters)
+ {
+ foreach (OdeCharacter actor in _characters)
+ {
+ if (actor != null)
+ {
+ if (actor.bad)
+ m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
+
+ actor.UpdatePositionAndVelocity();
+ }
+ }
+ }
+ */
+
+ lock (_activegroups)
{
- List defects = new List();
- foreach (OdeCharacter actor in _characters)
{
- if (actor != null)
- actor.Move(ODE_STEPSIZE, defects);
- }
- if (defects.Count != 0)
- {
- foreach (OdeCharacter defect in defects)
+ foreach (OdePrim actor in _activegroups)
{
- RemoveCharacter(defect);
+ if (actor.IsPhysical)
+ {
+ actor.UpdatePositionAndVelocity();
+ }
}
}
}
- statchmove += Util.EnvironmentTickCountSubtract(statstart);
-
- // Move other active objects
- lock (_activegroups)
- {
- foreach (OdePrim aprim in _activegroups)
- {
- aprim.Move();
- }
- }
-
- //ode.dunlock(world);
}
catch (Exception e)
{
@@ -1852,32 +1862,11 @@ namespace OpenSim.Region.Physics.OdePlugin
// ode.dunlock(world);
}
- d.JointGroupEmpty(contactgroup);
- totjcontact += m_global_contactcount;
step_time -= ODE_STEPSIZE;
nodeframes++;
}
- statstart = Util.EnvironmentTickCount();
-
-/*
-// now included in characters move() and done at ode rate
-// maybe be needed later if we need to do any extra work at hearbeat rate
- lock (_characters)
- {
- foreach (OdeCharacter actor in _characters)
- {
- if (actor != null)
- {
- if (actor.bad)
- m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
-
- actor.UpdatePositionAndVelocity();
- }
- }
- }
-*/
lock (_badCharacter)
{
if (_badCharacter.Count > 0)
@@ -1890,22 +1879,6 @@ namespace OpenSim.Region.Physics.OdePlugin
_badCharacter.Clear();
}
}
- statmovchar = Util.EnvironmentTickCountSubtract(statstart);
-
- lock (_activegroups)
- {
- {
- foreach (OdePrim actor in _activegroups)
- {
- if (actor.IsPhysical)
- {
- actor.UpdatePositionAndVelocity((float)nodeframes * ODE_STEPSIZE);
- }
- }
- }
- }
-
- statmovprim = Util.EnvironmentTickCountSubtract(statstart);
int nactivegeoms = d.SpaceGetNumGeoms(ActiveSpace);
int nstaticgeoms = d.SpaceGetNumGeoms(StaticSpace);