ubitODE - several changes...
UbitUmarov
parent
26fd1e3a0c
commit
e0f81e2400
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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,9 +3306,18 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
if (!childPrim && m_isphysical && Body != IntPtr.Zero &&
|
||||
!m_disabled && !m_isSelected && !m_building && !m_outbounds)
|
||||
{
|
||||
if (d.BodyIsEnabled(Body))
|
||||
if (!d.BodyIsEnabled(Body))
|
||||
{
|
||||
float timestep = _parent_scene.ODE_STEPSIZE;
|
||||
// let vehicles sleep
|
||||
if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
|
||||
return;
|
||||
|
||||
if (++bodydisablecontrol < 20)
|
||||
return;
|
||||
|
||||
bodydisablecontrol = 0;
|
||||
d.BodyEnable(Body);
|
||||
}
|
||||
|
||||
// check outside region
|
||||
d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator
|
||||
|
|
@ -3302,7 +3346,6 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
|
||||
base.RequestPhysicsterseUpdate();
|
||||
|
||||
m_throttleUpdates = false;
|
||||
throttleCounter = 0;
|
||||
_zeroFlag = true;
|
||||
|
||||
|
|
@ -3359,156 +3402,109 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
{
|
||||
// 'VEHICLES' are dealt with in ODEDynamics.cs
|
||||
m_vehicle.Step();
|
||||
return;
|
||||
}
|
||||
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 (m_usePID && m_PIDTau > 0)
|
||||
{
|
||||
|
||||
// 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);
|
||||
// for now position error
|
||||
_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)
|
||||
(m_PIDTarget.X - lpos.X),
|
||||
(m_PIDTarget.Y - lpos.Y),
|
||||
(m_PIDTarget.Z - lpos.Z)
|
||||
);
|
||||
|
||||
// if velocity is zero, use position control; otherwise, velocity control
|
||||
|
||||
if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
|
||||
if (_target_velocity.ApproxEquals(Vector3.Zero, 0.02f))
|
||||
{
|
||||
// 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);
|
||||
float tmp = 1 / m_PIDTau;
|
||||
_target_velocity *= tmp;
|
||||
|
||||
// vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
|
||||
// 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;
|
||||
}
|
||||
|
||||
fz = ((_target_velocity.Z - vel.Z) * (PID_D));
|
||||
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)
|
||||
else if (m_useHoverPID && m_PIDHoverTau != 0 && m_PIDHoverHeight != 0)
|
||||
{
|
||||
//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
|
||||
|
||||
m_groundHeight = _parent_scene.GetTerrainHeightAtXY(lpos.X, lpos.Y);
|
||||
|
||||
switch (m_PIDHoverType)
|
||||
{
|
||||
case PIDHoverType.Ground:
|
||||
m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
|
||||
|
||||
m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
|
||||
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;
|
||||
d.Vector3 vel = d.BodyGetLinearVel(Body);
|
||||
|
||||
// if velocity is zero, use position control; otherwise, velocity control
|
||||
fz = (m_targetHoverHeight - lpos.Z);
|
||||
|
||||
if (Math.Abs(fz) < 0.1f)
|
||||
// if error is zero, use position control; otherwise, velocity control
|
||||
if (Math.Abs(fz) < 0.01f)
|
||||
{
|
||||
// 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.BodySetPosition(Body, lpos.X, lpos.Y, m_targetHoverHeight);
|
||||
d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
|
||||
// ? d.BodyAddForce(Body, 0, 0, fz);
|
||||
return;
|
||||
}
|
||||
else
|
||||
{
|
||||
_zeroFlag = false;
|
||||
fz /= m_PIDHoverTau;
|
||||
|
||||
// We're flying and colliding with something
|
||||
fz = ((fz - vel.Z) * (PID_D));
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -3552,51 +3548,6 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
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
|
||||
{
|
||||
// let vehicles sleep
|
||||
if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
|
||||
return;
|
||||
|
||||
if (++bodydisablecontrol < 20)
|
||||
return;
|
||||
|
||||
bodydisablecontrol = 0;
|
||||
d.BodyEnable(Body);
|
||||
return;
|
||||
}
|
||||
}
|
||||
else
|
||||
{ // is not physical, or is not a body or is selected
|
||||
// _zeroPosition = d.BodyGetPosition(Body);
|
||||
|
|
@ -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;
|
||||
|
||||
// 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
|
||||
{
|
||||
_zeroFlag = false;
|
||||
m_lastUpdateSent = false;
|
||||
_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;
|
||||
|
||||
if (lastZeroFlag)
|
||||
{
|
||||
_velocity = Vector3.Zero;
|
||||
_acceleration = Vector3.Zero;
|
||||
m_rotationalVelocity = Vector3.Zero;
|
||||
}
|
||||
|
||||
if (!m_lastUpdateSent)
|
||||
{
|
||||
m_throttleUpdates = false;
|
||||
throttleCounter = 0;
|
||||
|
||||
base.RequestPhysicsterseUpdate();
|
||||
|
||||
if (lastZeroFlag)
|
||||
m_lastUpdateSent = true;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (lastZeroFlag != _zeroFlag)
|
||||
{
|
||||
base.RequestPhysicsterseUpdate();
|
||||
return;
|
||||
}
|
||||
|
||||
_position.X = lpos.X;
|
||||
_position.Y = lpos.Y;
|
||||
_position.Z = lpos.Z;
|
||||
|
||||
_orientation.X = ori.X;
|
||||
_orientation.Y = ori.Y;
|
||||
_orientation.Z = ori.Z;
|
||||
_orientation.W = ori.W;
|
||||
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;
|
||||
|
||||
_zeroFlag = true;
|
||||
|
||||
if (!m_lastUpdateSent)
|
||||
{
|
||||
m_throttleUpdates = false;
|
||||
throttleCounter = 0;
|
||||
|
||||
base.RequestPhysicsterseUpdate();
|
||||
|
||||
m_lastUpdateSent = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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
|
|||
/// <returns></returns>
|
||||
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<OdeCharacter> defects = new List<OdeCharacter>();
|
||||
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,57 +1822,12 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
}
|
||||
}
|
||||
|
||||
// do a ode simulation step
|
||||
d.WorldQuickStep(world, ODE_STEPSIZE);
|
||||
statstep += Util.EnvironmentTickCountSubtract(statstart);
|
||||
|
||||
// Move characters
|
||||
lock (_characters)
|
||||
{
|
||||
List<OdeCharacter> defects = new List<OdeCharacter>();
|
||||
foreach (OdeCharacter actor in _characters)
|
||||
{
|
||||
if (actor != null)
|
||||
actor.Move(ODE_STEPSIZE, defects);
|
||||
}
|
||||
if (defects.Count != 0)
|
||||
{
|
||||
foreach (OdeCharacter defect in defects)
|
||||
{
|
||||
RemoveCharacter(defect);
|
||||
}
|
||||
}
|
||||
}
|
||||
statchmove += Util.EnvironmentTickCountSubtract(statstart);
|
||||
|
||||
// Move other active objects
|
||||
lock (_activegroups)
|
||||
{
|
||||
foreach (OdePrim aprim in _activegroups)
|
||||
{
|
||||
aprim.Move();
|
||||
}
|
||||
}
|
||||
|
||||
//ode.dunlock(world);
|
||||
}
|
||||
catch (Exception e)
|
||||
{
|
||||
m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
|
||||
// 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
|
||||
// update managed ideia of physical data and do updates to core
|
||||
/*
|
||||
lock (_characters)
|
||||
{
|
||||
foreach (OdeCharacter actor in _characters)
|
||||
|
|
@ -1877,7 +1841,32 @@ namespace OpenSim.Region.Physics.OdePlugin
|
|||
}
|
||||
}
|
||||
}
|
||||
*/
|
||||
*/
|
||||
|
||||
lock (_activegroups)
|
||||
{
|
||||
{
|
||||
foreach (OdePrim actor in _activegroups)
|
||||
{
|
||||
if (actor.IsPhysical)
|
||||
{
|
||||
actor.UpdatePositionAndVelocity();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
catch (Exception e)
|
||||
{
|
||||
m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
|
||||
// ode.dunlock(world);
|
||||
}
|
||||
|
||||
|
||||
step_time -= ODE_STEPSIZE;
|
||||
nodeframes++;
|
||||
}
|
||||
|
||||
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);
|
||||
|
|
|
|||
Loading…
Reference in New Issue