public void Vector3ApproxEquals()
{
Vector3 a = new Vector3(1f, 0f, 0f);
Vector3 b = new Vector3(0f, 0f, 0f);
Assert.IsFalse(a.ApproxEquals(b, 0.9f), "ApproxEquals failed (1)");
Assert.IsTrue(a.ApproxEquals(b, 1.0f), "ApproxEquals failed (2)");
a = new Vector3(-1f, 0f, 0f);
b = new Vector3(1f, 0f, 0f);
Assert.IsFalse(a.ApproxEquals(b, 1.9f), "ApproxEquals failed (3)");
Assert.IsTrue(a.ApproxEquals(b, 2.0f), "ApproxEquals failed (4)");
a = new Vector3(0f, -1f, 0f);
b = new Vector3(0f, -1.1f, 0f);
Assert.IsFalse(a.ApproxEquals(b, 0.09f), "ApproxEquals failed (5)");
Assert.IsTrue(a.ApproxEquals(b, 0.11f), "ApproxEquals failed (6)");
a = new Vector3(0f, 0f, 0.00001f);
b = new Vector3(0f, 0f, 0f);
Assert.IsFalse(b.ApproxEquals(a, Single.Epsilon), "ApproxEquals failed (6)");
Assert.IsTrue(b.ApproxEquals(a, 0.0001f), "ApproxEquals failed (7)");
}
/// <summary>
/// Called from Simulate
/// This is the avatar's movement control + PID Controller
/// </summary>
/// <param name="timeStep"></param>
public void Move (float timeStep, ref List<AuroraODECharacter> defects)
{
// 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 (Body == IntPtr.Zero)
return;
if (!m_shouldBePhysical)
return;
// replace amotor
d.Quaternion dtmp;
dtmp.W = 1;
dtmp.X = 0;
dtmp.Y = 0;
dtmp.Z = 0;
d.BodySetQuaternion (Body, ref dtmp);
d.BodySetAngularVel(Body, 0, 0, 0);
Vector3 vec = Vector3.Zero;
d.Vector3 vel = d.BodyGetLinearVel(Body);
#region Flight Ceiling
// rex, added height check
d.Vector3 tempPos = d.BodyGetPosition (Body);
if (m_pidControllerActive == false)
{
_zeroPosition = d.BodyGetPosition (Body);
}
if (_parent_scene.m_useFlightCeilingHeight && tempPos.Z > _parent_scene.m_flightCeilingHeight)
{
tempPos.Z = _parent_scene.m_flightCeilingHeight;
d.BodySetPosition (Body, tempPos.X, tempPos.Y, tempPos.Z);
d.Vector3 tempVel = d.BodyGetLinearVel (Body);
if (tempVel.Z > 0.0f)
{
tempVel.Z = 0.0f;
d.BodySetLinearVel (Body, tempVel.X, tempVel.Y, tempVel.Z);
}
if (_target_velocity.Z > 0.0f)
_target_velocity.Z = 0.0f;
}
// endrex
#endregion
#region NonFinite Pos
Vector3 localPos = new Vector3 ((float)tempPos.X, (float)tempPos.Y, (float)tempPos.Z);
if (!localPos.IsFinite ())
{
m_log.Warn ("[PHYSICS]: Avatar Position is non-finite!");
defects.Add (this);
// _parent_scene.RemoveCharacter(this);
// destroy avatar capsule and related ODE data
/*
if (Amotor != IntPtr.Zero)
{
// Kill the Amotor
d.JointDestroy(Amotor);
Amotor = IntPtr.Zero;
}
*/
//kill the Geometry
_parent_scene.waitForSpaceUnlock (_parent_scene.space);
if (Body != IntPtr.Zero)
{
//kill the body
d.BodyDestroy (Body);
Body = IntPtr.Zero;
}
if (Shell != IntPtr.Zero)
{
d.GeomDestroy (Shell);
_parent_scene.geom_name_map.Remove (Shell);
Shell = IntPtr.Zero;
}
return;
}
#endregion
#region Check for out of region
if (Position.X < 0.25f || Position.Y < 0.25f ||
Position.X > _parent_scene.Region.RegionSizeX - .25f ||
Position.Y > _parent_scene.Region.RegionSizeY - .25f)
{
if (!CheckForRegionCrossing())
{
Vector3 newPos = Position;
newPos.X = Util.Clip(Position.X, 0.75f, _parent_scene.Region.RegionSizeX - 0.75f);
newPos.Y = Util.Clip(Position.Y, 0.75f, _parent_scene.Region.RegionSizeY - 0.75f);
Position = newPos;
d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z);
}
}
#endregion
#region Movement Multiplier
float movementmult = 1f;
if (!m_alwaysRun)
movementmult /= _parent_scene.avMovementDivisorWalk;
else
movementmult /= _parent_scene.avMovementDivisorRun;
movementmult *= 10;
movementmult *= SpeedModifier;
if (flying)
movementmult *= _parent_scene.m_AvFlySpeed;
#endregion
#region Check for underground
if (!(Position.X < 0.25f || Position.Y < 0.25f ||
Position.X > _parent_scene.Region.RegionSizeX - .25f ||
Position.Y > _parent_scene.Region.RegionSizeY - .25f))
{
float groundHeight = _parent_scene.GetTerrainHeightAtXY (
tempPos.X + (tempPos.X == 0 ? tempPos.X : timeStep * 0.75f * vel.X),
tempPos.Y + (tempPos.Y == 0 ? tempPos.Y : timeStep * 0.75f * vel.Y));
if ((tempPos.Z - AvatarHalfsize) < groundHeight)
{
if (!flying)
{
//if (_target_velocity.Z < 0)
_target_velocity.Z = 0;
vec.Z = -vel.Z * PID_D * 2f + ((groundHeight - (tempPos.Z - AvatarHalfsize)) * PID_P * 100.0f);
}
else
vec.Z = ((groundHeight - (tempPos.Z - AvatarHalfsize)) * PID_P);
}
if (tempPos.Z - AvatarHalfsize - groundHeight < 0.12f)
{
m_iscolliding = true;
m_iscollidingGround = true;
flying = false; // ground the avatar
ContactPoint point = new ContactPoint ();
point.PenetrationDepth = vel.Z;
point.Position = Position;
point.SurfaceNormal = Vector3.Zero;
//0 is the ground localID
this.AddCollisionEvent (0, point);
}
else
m_iscollidingGround = false;
}
else
m_iscollidingGround = true;//Assume that they are crossing, and keep them from falling down
#endregion
#region Movement
#region Jump code
if (IsJumping && (IsColliding && m_preJumpCounter > _parent_scene.m_preJumpTime + 10) || m_preJumpCounter > 50)
{
m_isJumping = false;
m_preJumpCounter = 0;
}
if (IsJumping)
m_preJumpCounter++;
if (m_ispreJumping && m_preJumpCounter == _parent_scene.m_preJumpTime)
{
m_ispreJumping = false;
_target_velocity += m_preJumpForce * _parent_scene.m_preJumpForceMultiplier;
m_preJumpCounter = 0;
m_isJumping = true;
}
if (m_ispreJumping)
{
m_preJumpCounter++;
TriggerMovementUpdate();
return;
}
#endregion
// if velocity is zero, use position control; otherwise, velocity control
if (_target_velocity == Vector3.Zero &&
Math.Abs (vel.X) < 0.05 && Math.Abs (vel.Y) < 0.05 && Math.Abs (vel.Z) < 0.05 && (this.m_iscollidingGround || this.m_iscollidingObj || this.flying))
//This is so that if we get moved by something else, it will update us in the client
{
m_isJumping = false;
// keep track of where we stopped. No more slippin' & slidin'
if (!_zeroFlag)
{
_zeroFlag = true;
_zeroPosition = tempPos;
}
if (m_pidControllerActive)
{
// 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
// if target vel is zero why was it here ?
vec.X = -vel.X * PID_D + (_zeroPosition.X - tempPos.X) * PID_P;
vec.Y = -vel.Y * PID_D + (_zeroPosition.Y - tempPos.Y) * PID_P;
}
}
else
{
m_pidControllerActive = true;
_zeroFlag = false;
if (m_iscolliding)
{
if (!flying)
{
if (_target_velocity.Z != 0.0f)
vec.Z = (_target_velocity.Z * movementmult - vel.Z) * PID_D;// + (_zeroPosition.Z - tempPos.Z) * PID_P)) _zeropos maybe bad here
// We're standing or walking on something
vec.X = (_target_velocity.X * movementmult - vel.X) * PID_D * 2;
vec.Y = (_target_velocity.Y * movementmult - vel.Y) * PID_D * 2;
}
else
{
// We're flying and colliding with something
vec.X = (_target_velocity.X * movementmult - vel.X) * PID_D * 0.5f;
vec.Y = (_target_velocity.Y * movementmult - vel.Y) * PID_D * 0.5f;
}
}
else
{
if (flying)
{
// we're flying
vec.X = (_target_velocity.X * movementmult - vel.X) * PID_D * 0.75f;
vec.Y = (_target_velocity.Y * movementmult - vel.Y) * PID_D * 0.75f;
}
else
{
// we're not colliding and we're not flying so that means we're falling!
// m_iscolliding includes collisions with the ground.
vec.X = (_target_velocity.X - vel.X) * PID_D * 0.85f;
vec.Y = (_target_velocity.Y - vel.Y) * PID_D * 0.85f;
}
}
if (flying)
{
#region Av gravity
if (_parent_scene.AllowAvGravity &&
tempPos.Z > _parent_scene.AvGravityHeight) //Should be stop avies from flying upwards
{
//Decay going up
if (_target_velocity.Z > 0)
{
//How much should we force them down?
float Multiplier = (_parent_scene.AllowAvsToEscapeGravity ? .03f : .1f);
//How much should we force them down?
float fudgeHeight = (_parent_scene.AllowAvsToEscapeGravity ? 80 : 30);
//We add the 30 so that gravity is resonably strong once they pass the min height
Multiplier *= tempPos.Z + fudgeHeight - _parent_scene.AvGravityHeight;
//Limit these so that things don't go wrong
if (Multiplier < 1)
Multiplier = 1;
float maxpower = (_parent_scene.AllowAvsToEscapeGravity ? 1.5f : 3f);
if (Multiplier > maxpower)
Multiplier = maxpower;
_target_velocity.Z /= Multiplier;
vel.Z /= Multiplier;
}
}
#endregion
vec.Z = (_target_velocity.Z * movementmult - vel.Z) * PID_D * 0.5f;
if (_parent_scene.AllowAvGravity && tempPos.Z > _parent_scene.AvGravityHeight)
//Add extra gravity
vec.Z += ((10 * _parent_scene.gravityz) * Mass);
}
}
if (flying)
{
#region Auto Fly Height
//Added for auto fly height. Kitto Flora
//Changed to only check if the avatar is flying around,
// Revolution: If the avatar is going down, they are trying to land (probably), so don't push them up to make it harder
// Only if they are moving around sideways do we need to push them up
if (_target_velocity.X != 0 || _target_velocity.Y != 0)
{
Vector3 forwardVel = new Vector3 (_target_velocity.X > 0 ? 2 : (_target_velocity.X < 0 ? -2 : 0),
_target_velocity.Y > 0 ? 2 : (_target_velocity.Y < 0 ? -2 : 0),
0);
float target_altitude = _parent_scene.GetTerrainHeightAtXY (tempPos.X, tempPos.Y) + MinimumGroundFlightOffset;
//We cheat a bit and do a bit lower than normal
if ((tempPos.Z - CAPSULE_LENGTH) < target_altitude ||
(tempPos.Z - CAPSULE_LENGTH) < _parent_scene.GetTerrainHeightAtXY (tempPos.X + forwardVel.X, tempPos.Y + forwardVel.Y)
+ MinimumGroundFlightOffset)
vec.Z += (target_altitude - tempPos.Z) * PID_P * 0.5f;
}
else
{
//Straight up and down, only apply when they are very close to the ground
float target_altitude = _parent_scene.GetTerrainHeightAtXY (tempPos.X, tempPos.Y);
if ((tempPos.Z - CAPSULE_LENGTH + (MinimumGroundFlightOffset / 1.5)) < target_altitude + MinimumGroundFlightOffset)
{
if ((tempPos.Z - CAPSULE_LENGTH) < target_altitude + 1)
{
vec.Z += ((target_altitude + 4) - (tempPos.Z - CAPSULE_LENGTH)) * PID_P;
}
else
vec.Z += ((target_altitude + MinimumGroundFlightOffset) - (tempPos.Z - CAPSULE_LENGTH)) * PID_P * 0.5f;
}
}
#endregion
}
#region Gravity
if (!flying && _parent_scene.AllowAvGravity)
{
if (!_parent_scene.UsePointGravity)
{
//Add normal gravity
vec.X += _parent_scene.gravityx * m_mass;
vec.Y += _parent_scene.gravityy * m_mass;
vec.Z += _parent_scene.gravityz * m_mass;
}
else
{
Vector3 cog = _parent_scene.PointOfGravity;
if (cog.X != 0)
vec.X += (cog.X - tempPos.X) * m_mass;
if (cog.Y != 0)
vec.Y += (cog.Y - tempPos.Y) * m_mass;
if (cog.Z != 0)
vec.Z += (cog.Z - tempPos.Z) * m_mass;
}
}
#endregion
#region Under water physics
if (_parent_scene.AllowUnderwaterPhysics && (float)tempPos.X < _parent_scene.Region.RegionSizeX &&
(float)tempPos.Y < _parent_scene.Region.RegionSizeY)
{
//Position plus height to av's shoulder (aprox) is just above water
if ((tempPos.Z + (CAPSULE_LENGTH / 3) - .25f) < _parent_scene.GetWaterLevel ((float)tempPos.X, (float)tempPos.Y))
{
if (StartingUnderWater)
ShouldBeWalking = Flying == false;
StartingUnderWater = false;
WasUnderWater = true;
Flying = true;
lastUnderwaterPush = 0;
if (ShouldBeWalking)
{
lastUnderwaterPush += (float)(_parent_scene.GetWaterLevel ((float)tempPos.X, (float)tempPos.Y) - tempPos.Z) * 33 + 3;
vec.Z += lastUnderwaterPush;
}
else
{
lastUnderwaterPush += 3500;
lastUnderwaterPush += (float)(_parent_scene.GetWaterLevel ((float)tempPos.X, (float)tempPos.Y) - tempPos.Z) * 8;
vec.Z += lastUnderwaterPush;
}
}
else
{
StartingUnderWater = true;
if (WasUnderWater)
{
WasUnderWater = false;
Flying = true;
}
}
}
#endregion
#endregion
#region Apply the force
if (vec.IsFinite ())
{
if (vec.X < 100000000 && vec.Y < 10000000 && vec.Z < 10000000) //Checks for crazy, going to NaN us values
{
// round small values to zero. those possible are just errors
if (Math.Abs (vec.X) < 0.001)
vec.X = 0;
if (Math.Abs (vec.Y) < 0.001)
vec.Y = 0;
if (Math.Abs (vec.Z) < 0.001)
vec.Z = 0;
if (vec == Vector3.Zero) //if we arn't moving, STOP
d.BodySetLinearVel(Body, vec.X, vec.Y, vec.Z);
else
doForce (vec);
//When falling, we keep going faster and faster, and eventually, the client blue screens (blue is all you see).
// The speed that does this is slightly higher than -30, so we cap it here so we never do that during falling.
if (vel.Z < -30)
{
vel.Z = -30;
d.BodySetLinearVel (Body, vel.X, vel.Y, vel.Z);
}
//Decay out the target velocity
_target_velocity *= _parent_scene.m_avDecayTime;
if (!_zeroFlag && _target_velocity.ApproxEquals (Vector3.Zero, _parent_scene.m_avStopDecaying))
_target_velocity = Vector3.Zero;
}
else
{
//This is a safe guard from going NaN, but it isn't very smooth... which is ok
d.BodySetForce (Body, 0, 0, 0);
d.BodySetLinearVel (Body, 0, 0, 0);
}
}
else
{
m_log.Warn ("[PHYSICS]: Got a NaN force vector in Move()");
m_log.Warn ("[PHYSICS]: Avatar Position is non-finite!");
defects.Add (this);
//kill the Geometry
_parent_scene.waitForSpaceUnlock (_parent_scene.space);
if (Body != IntPtr.Zero)
{
//kill the body
d.BodyDestroy (Body);
Body = IntPtr.Zero;
}
if (Shell != IntPtr.Zero)
{
d.GeomDestroy (Shell);
_parent_scene.geom_name_map.Remove (Shell);
Shell = IntPtr.Zero;
}
}
#endregion
}
} // end MoveLinear()
private void MoveAngular (float pTimestep, AuroraODEPhysicsScene _pParentScene, AuroraODEPrim parent)
{
d.Vector3 angularVelocity = d.BodyGetAngularVel (Body);
d.Quaternion rot = d.BodyGetQuaternion (Body);
Quaternion rotq = new Quaternion (rot.X, rot.Y, rot.Z, rot.W);
// Vector3 angularVelocity = Vector3.Zero;
/*if ((m_flags & VehicleFlag.MOUSELOOK_STEER) == VehicleFlag.MOUSELOOK_STEER)
{
if (m_userLookAt != Quaternion.Identity)
{
Quaternion camrot = Quaternion.Subtract (m_userLookAt, rotq);
camrot.Normalize ();
m_angularMotorVelocity += Vector3.One * camrot;
Console.WriteLine (Vector3.One * camrot);
}
}*/
if (m_angularMotorApply > 0)
{
// ramp up to new value
// current velocity += error / (time to get there / step interval)
// requested speed - last motor speed
m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep);
m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep);
m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep);
m_angularMotorApply--; // This is done so that if script request rate is less than phys frame rate the expected
// velocity may still be acheived.
}
else
{
// no motor recently applied, keep the body velocity
/* m_angularMotorVelocity.X = angularVelocity.X;
m_angularMotorVelocity.Y = angularVelocity.Y;
m_angularMotorVelocity.Z = angularVelocity.Z; */
// and decay the velocity
m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / (pTimestep * pTimestep));
} // end motor section
// Vertical attractor section
Vector3 vertattr = Vector3.Zero;
Vector3 deflection = Vector3.Zero;
Vector3 banking = Vector3.Zero;
if (m_verticalAttractionTimescale < 300)
{
float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep);
// get present body rotation
// make a vector pointing up
Vector3 verterr = Vector3.Zero;
verterr.Z = 1.0f;
// rotate it to Body Angle
verterr = verterr * rotq;
// verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1.
// As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go
// negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
if (verterr.Z < 0.0f)
{
verterr.X = 2.0f - verterr.X;
verterr.Y = 2.0f - verterr.Y;
}
// Error is 0 (no error) to +/- 2 (max error)
// scale it by VAservo
verterr = verterr * VAservo;
//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
// Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
vertattr.X = verterr.Y;
vertattr.Y = -verterr.X;
vertattr.Z = 0f;
// scaling appears better usingsquare-law
float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
vertattr.X += bounce * angularVelocity.X;
vertattr.Y += bounce * angularVelocity.Y;
} // else vertical attractor is off
// m_lastVertAttractor = vertattr;
// Bank section tba
#region Deflection
//Forward is the prefered direction, but if the reference frame has changed, we need to take this into account as well
Vector3 PreferredAxisOfMotion = new Vector3 ((10 * (m_angularDeflectionEfficiency / m_angularDeflectionTimescale) * pTimestep), 0, 0);
PreferredAxisOfMotion *= Quaternion.Add(rotq, m_referenceFrame);
//Multiply it so that it scales linearly
//deflection = PreferredAxisOfMotion;
//deflection = ((PreferredAxisOfMotion * m_angularDeflectionEfficiency) / (m_angularDeflectionTimescale / pTimestep));
#endregion
#region Banking
if (m_bankingEfficiency != 0)
{
Vector3 angularMotorVelocity = new Vector3 ();
if (m_angularMotorApply > 0)
{
// ramp up to new value
// current velocity += error / (time to get there / step interval)
// requested speed - last motor speed
angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep);
angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep);
angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep);
// velocity may still be acheived.
Vector3 dir = Vector3.One * rotq;
float mult = /*dir.X > 0 ?*/ -1f /*: 1*/;
mult *= m_bankingMix;//Changes which way it banks in and out of turns
//Use the square of the efficiency, as it looks much more how SL banking works
float effSquared = (m_bankingEfficiency * m_bankingEfficiency);
if (m_bankingEfficiency < 0)
effSquared *= -1;//Keep the negative!
banking.Z += (effSquared * (mult)) * (angularMotorVelocity.X);
m_angularMotorVelocity.X *= 1 - m_bankingEfficiency;
if (Math.Abs(m_lastAngularVelocity.Z) > m_bankingMix)
{
Vector3 bankingRot = new Vector3 (m_lastAngularVelocity.Z * (effSquared * 10 * (m_bankingMix * (-1))), 0, 0);
bankingRot *= rotq;
banking += bankingRot;
//m_angularMotorDirection.Y = m_lastAngularVelocity.Z * (m_bankingEfficiency);
//m_linearMotorDirectionLASTSET.Y = m_lastAngularVelocity.Z * (m_bankingEfficiency * 100);
}
}
}
#endregion
// Sum velocities
m_lastAngularVelocity = m_angularMotorVelocity + vertattr + deflection + banking;
if (!m_lastAngularVelocity.ApproxEquals (Vector3.Zero, 0.01f))
{
if (!d.BodyIsEnabled (Body))
d.BodyEnable (Body);
}
else
{
m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
}
#region Linear Motor Offset
//Offset section
if (m_linearMotorOffset != Vector3.Zero)
{
//Offset of linear velocity doesn't change the linear velocity,
// but causes a torque to be applied, for example...
//
// IIIII >>> IIIII
// IIIII >>> IIIII
// IIIII >>> IIIII
// ^
// | Applying a force at the arrow will cause the object to move forward, but also rotate
//
//
// The torque created is the linear velocity crossed with the offset
//Note: we use the motor, otherwise you will just spin around and we divide by 10 since otherwise we go crazy
Vector3 torqueFromOffset = (m_linearMotorDirectionLASTSET % m_linearMotorOffset) / 10;
d.BodyAddTorque (Body, torqueFromOffset.X, torqueFromOffset.Y, torqueFromOffset.Z);
}
#endregion
/*if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
{
m_lastAngularVelocity.X = 0;
m_lastAngularVelocity.Y = 0;
}*/
// apply friction
Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep);
m_lastAngularVelocity -= m_lastAngularVelocity * decayamount;
// Apply to the body
d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
}
public void TestLinkDelink2groups4SceneObjects()
{
TestHelpers.InMethod();
bool debugtest = false;
Scene scene = SceneHelpers.SetupScene();
SceneObjectPart part1 = SceneHelpers.AddSceneObject(scene);
SceneObjectGroup grp1 = part1.ParentGroup;
SceneObjectPart part2 = SceneHelpers.AddSceneObject(scene);
SceneObjectGroup grp2 = part2.ParentGroup;
SceneObjectPart part3 = SceneHelpers.AddSceneObject(scene);
SceneObjectGroup grp3 = part3.ParentGroup;
SceneObjectPart part4 = SceneHelpers.AddSceneObject(scene);
SceneObjectGroup grp4 = part4.ParentGroup;
grp1.AbsolutePosition = new Vector3(10, 10, 10);
grp2.AbsolutePosition = Vector3.Zero;
grp3.AbsolutePosition = new Vector3(20, 20, 20);
grp4.AbsolutePosition = new Vector3(40, 40, 40);
// <90,0,0>
// grp1.UpdateGroupRotationR(Quaternion.CreateFromEulers(90 * Utils.DEG_TO_RAD, 0, 0));
// <180,0,0>
grp2.UpdateGroupRotationR(Quaternion.CreateFromEulers(180 * Utils.DEG_TO_RAD, 0, 0));
// <270,0,0>
// grp3.UpdateGroupRotationR(Quaternion.CreateFromEulers(270 * Utils.DEG_TO_RAD, 0, 0));
// <0,90,0>
grp4.UpdateGroupRotationR(Quaternion.CreateFromEulers(0, 90 * Utils.DEG_TO_RAD, 0));
// Required for linking
grp1.RootPart.ClearUpdateSchedule();
grp2.RootPart.ClearUpdateSchedule();
grp3.RootPart.ClearUpdateSchedule();
grp4.RootPart.ClearUpdateSchedule();
// Link grp2 to grp1. part2 becomes child prim to grp1. grp2 is eliminated.
grp1.LinkToGroup(grp2);
// Link grp4 to grp3.
grp3.LinkToGroup(grp4);
// At this point we should have 4 parts total in two groups.
Assert.That(grp1.Parts.Length == 2, "Group1 children count should be 2");
Assert.That(grp2.IsDeleted, "Group 2 was not registered as deleted after link.");
Assert.That(grp2.Parts.Length, Is.EqualTo(0), "Group 2 still contained parts after delink.");
Assert.That(grp3.Parts.Length == 2, "Group3 children count should be 2");
Assert.That(grp4.IsDeleted, "Group 4 was not registered as deleted after link.");
Assert.That(grp4.Parts.Length, Is.EqualTo(0), "Group 4 still contained parts after delink.");
if (debugtest)
{
m_log.Debug("--------After Link-------");
m_log.Debug("Group1: parts:" + grp1.Parts.Length);
m_log.Debug("Group1: Pos:"+grp1.AbsolutePosition+", Rot:"+grp1.GroupRotation);
m_log.Debug("Group1: Prim1: OffsetPosition:" + part1.OffsetPosition + ", OffsetRotation:" + part1.RotationOffset);
m_log.Debug("Group1: Prim2: OffsetPosition:"+part2.OffsetPosition+", OffsetRotation:"+ part2.RotationOffset);
m_log.Debug("Group3: parts:" + grp3.Parts.Length);
m_log.Debug("Group3: Pos:"+grp3.AbsolutePosition+", Rot:"+grp3.GroupRotation);
m_log.Debug("Group3: Prim1: OffsetPosition:"+part3.OffsetPosition+", OffsetRotation:"+part3.RotationOffset);
m_log.Debug("Group3: Prim2: OffsetPosition:"+part4.OffsetPosition+", OffsetRotation:"+part4.RotationOffset);
}
// Required for linking
grp1.RootPart.ClearUpdateSchedule();
grp3.RootPart.ClearUpdateSchedule();
// root part should have no offset position or rotation
Assert.That(part1.OffsetPosition == Vector3.Zero && part1.RotationOffset == Quaternion.Identity,
"root part should have no offset position or rotation (again)");
// offset position should be root part position - part2.absolute position.
Assert.That(part2.OffsetPosition == new Vector3(-10, -10, -10),
"offset position should be root part position - part2.absolute position (again)");
float roll = 0;
float pitch = 0;
float yaw = 0;
// There's a euler anomoly at 180, 0, 0 so expect 180 to turn into -180.
part1.RotationOffset.GetEulerAngles(out roll, out pitch, out yaw);
Vector3 rotEuler1 = new Vector3(roll * Utils.RAD_TO_DEG, pitch * Utils.RAD_TO_DEG, yaw * Utils.RAD_TO_DEG);
if (debugtest)
m_log.Debug(rotEuler1);
part2.RotationOffset.GetEulerAngles(out roll, out pitch, out yaw);
Vector3 rotEuler2 = new Vector3(roll * Utils.RAD_TO_DEG, pitch * Utils.RAD_TO_DEG, yaw * Utils.RAD_TO_DEG);
if (debugtest)
m_log.Debug(rotEuler2);
Assert.That(rotEuler2.ApproxEquals(new Vector3(-180, 0, 0), 0.001f) || rotEuler2.ApproxEquals(new Vector3(180, 0, 0), 0.001f),
"Not sure what this assertion is all about...");
// Now we're linking the first group to the third group. This will make the first group child parts of the third one.
grp3.LinkToGroup(grp1);
// Delink parts 2 and 3
grp3.DelinkFromGroup(part2.LocalId);
grp3.DelinkFromGroup(part3.LocalId);
if (debugtest)
{
m_log.Debug("--------After De-Link-------");
m_log.Debug("Group1: parts:" + grp1.Parts.Length);
m_log.Debug("Group1: Pos:" + grp1.AbsolutePosition + ", Rot:" + grp1.GroupRotation);
m_log.Debug("Group1: Prim1: OffsetPosition:" + part1.OffsetPosition + ", OffsetRotation:" + part1.RotationOffset);
m_log.Debug("Group1: Prim2: OffsetPosition:" + part2.OffsetPosition + ", OffsetRotation:" + part2.RotationOffset);
m_log.Debug("Group3: parts:" + grp3.Parts.Length);
m_log.Debug("Group3: Pos:" + grp3.AbsolutePosition + ", Rot:" + grp3.GroupRotation);
m_log.Debug("Group3: Prim1: OffsetPosition:" + part3.OffsetPosition + ", OffsetRotation:" + part3.RotationOffset);
m_log.Debug("Group3: Prim2: OffsetPosition:" + part4.OffsetPosition + ", OffsetRotation:" + part4.RotationOffset);
}
Assert.That(part2.AbsolutePosition == Vector3.Zero, "Badness 1");
Assert.That(part4.OffsetPosition == new Vector3(20, 20, 20), "Badness 2");
Quaternion compareQuaternion = new Quaternion(0, 0.7071068f, 0, 0.7071068f);
Assert.That((part4.RotationOffset.X - compareQuaternion.X < 0.00003)
&& (part4.RotationOffset.Y - compareQuaternion.Y < 0.00003)
&& (part4.RotationOffset.Z - compareQuaternion.Z < 0.00003)
&& (part4.RotationOffset.W - compareQuaternion.W < 0.00003),
"Badness 3");
}
public bool GetNextPosition(Vector3 currentPosition, float closeToRange, int secondsBeforeForcedTeleport,
out Vector3 position, out TravelMode state, out bool needsToTeleportToPosition)
{
const bool found = false;
lock (m_lock)
{
findNewTarget:
position = Vector3.Zero;
state = TravelMode.None;
needsToTeleportToPosition = false;
if ((m_listOfPositions.Count - m_currentpos) > 0)
{
position = m_listOfPositions[m_currentpos];
state = m_listOfStates[m_currentpos];
if (state != TravelMode.Wait && state != TravelMode.TriggerHereEvent && position.ApproxEquals(currentPosition, closeToRange))
{
//Its close to a position, go look for the next pos
m_currentpos++;
m_lastChangedPosition = DateTime.MinValue;
goto findNewTarget;
}
if (state == TravelMode.TriggerHereEvent)
{
m_currentpos++; //Clear for next time, as we only fire this one time
m_lastChangedPosition = DateTime.MinValue;
}
else if (state == TravelMode.Wait)
{
if (m_waitingSince == DateTime.MinValue)
m_waitingSince = DateTime.Now;
else
{
if ((DateTime.Now - m_waitingSince).Seconds > position.X)
{
m_waitingSince = DateTime.MinValue;
m_currentpos++;
m_lastChangedPosition = DateTime.MinValue;
goto findNewTarget;
}
}
}
else
{
m_lastChangedPosition = DateTime.Now;
if ((DateTime.Now - m_lastChangedPosition).Seconds > secondsBeforeForcedTeleport)
needsToTeleportToPosition = true;
}
return true;
}
if (m_listOfPositions.Count == 0)
return false;
if (FollowIndefinitely)
{
m_currentpos = 0; //Reset the position to the beginning if we have run out of positions
goto findNewTarget;
}
}
return found;
}
public void Move(float timestep)
{
if (m_frozen)
return;
float fx = 0;
float fy = 0;
float fz = 0;
if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims.
{
if (m_vehicle.Type != Vehicle.TYPE_NONE)
{
// 'VEHICLES' are dealt with in ODEDynamics.cs
m_vehicle.Step(Body, timestep, _parent_scene, this);
d.Vector3 vel = d.BodyGetLinearVel(Body);
_velocity = new Vector3((float)vel.X, (float)vel.Y, (float)vel.Z);
d.Vector3 pos = d.BodyGetPosition(Body);
_position = new Vector3((float)pos.X, (float)pos.Y, (float)pos.Z);
_zeroFlag = false;
}
else
{
//Console.WriteLine("Move " + m_primName);
//if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009
// NON-'VEHICLES' are dealt with here
//float PID_P = 900.0f;
float m_mass = Mass;
// fz = 0f;
//m_log.Info(m_collisionFlags.ToString());
//KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
// would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ??
// m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
// gravityz multiplier = 1 - m_buoyancy
if (!_parent_scene.UsePointGravity)
{
if (!testRealGravity)
{
fx = _parent_scene.gravityx * (1.0f - m_buoyancy) * m_mass;
fy = _parent_scene.gravityy * (1.0f - m_buoyancy) * m_mass;
fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass;
}
else
{
fx = _parent_scene.gravityx * -1 * (m_buoyancy);
fy = _parent_scene.gravityy * -1 * (m_buoyancy);
fz = _parent_scene.gravityz * -1 * (m_buoyancy);
}
}
else
{
//Set up point gravity for this object
Vector3 cog = _parent_scene.PointOfGravity;
if (cog.X != 0)
fx = (cog.X - Position.X) * m_mass;
if (cog.Y != 0)
fy = (cog.Y - Position.Y) * m_mass;
if (cog.Z != 0)
fz = (cog.Z - Position.Z) * m_mass;
}
d.Vector3 vel = d.BodyGetLinearVel(Body);
d.Vector3 force = d.BodyGetForce(Body);
d.Vector3 angvel = d.BodyGetAngularVel(Body);
#region PID
if (m_usePID)
{
//Console.WriteLine("PID " + m_primName);
// KF - this is for object move? eg. llSetPos() ?
//if (!d.BodyIsEnabled(Body))
//d.BodySetForce(Body, 0f, 0f, 0f);
// If we're using the PID controller, then we have no gravity
//fz = (-1 * _parent_scene.gravityz) * m_mass; //KF: ?? Prims have no global gravity,so simply...
fz = 0f;
// 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) && (m_PIDTau != 0))
{
//PID_G = PID_G / m_PIDTau;
m_PIDTau = 1;
}
if ((PID_G - m_PIDTau) <= 0)
{
PID_G = m_PIDTau + 1;
}
//PidStatus = true;
// PhysicsVector vec = new PhysicsVector();
d.Vector3 pos = d.BodyGetPosition(Body);
_target_velocity =
new Vector3(
(float)(m_PIDTarget.X - pos.X) * ((/*PID_G - */m_PIDTau) * timestep),
(float)(m_PIDTarget.Y - pos.Y) * ((/*PID_G - */m_PIDTau) * timestep),
(float)(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);
if (!m_angularlock.ApproxEquals(Vector3.One, 0.003f) &&
Amotor != IntPtr.Zero)
{
}
d.BodySetLinearVel(Body, 0, 0, 0);
d.BodyAddForce(Body, 0, 0, fz);
return;
}
else
{
_zeroFlag = false;
// We're flying and colliding with something
fx = (float)((_target_velocity.X) - vel.X) * (PID_D);
fy = (float)((_target_velocity.Y) - vel.Y) * (PID_D);
// vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
fz = (float)(fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass));
}
} // end if (m_usePID)
#endregion
#region Hover PID
// Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
if (m_useHoverPID && !m_usePID)
{
//Console.WriteLine("Hover " + m_primName);
// If we're using the PID controller, then we have no gravity
fx = (-1 * _parent_scene.gravityx) * m_mass;
fy = (-1 * _parent_scene.gravityy) * m_mass;
fz = (-1 * _parent_scene.gravityz) * m_mass;
// 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);
// 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((float)pos.X, (float)pos.Y);
m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
break;
case PIDHoverType.GroundAndWater:
m_groundHeight = _parent_scene.GetTerrainHeightAtXY((float)pos.X, (float)pos.Y);
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)
_target_velocity =
new Vector3(0.0f, 0.0f,
(float)(m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * 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
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 = (float)(fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass));
}
}
#endregion
fx *= m_mass;
fy *= m_mass;
fx += m_force.X;
fy += m_force.Y;
fz += m_force.Z;
//m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
if (fx != 0 || fy != 0 || fz != 0)
{
// 35n times the mass per second applied maximum.
float nmax = 35f * m_mass;
float nmin = -35f * m_mass;
if (fx > nmax)
fx = nmax;
if (fx < nmin)
fx = nmin;
if (fy > nmax)
fy = nmax;
if (fy < nmin)
fy = nmin;
if (!m_angularlock.ApproxEquals(Vector3.One, 0.003f) &&
Amotor != IntPtr.Zero)
{
d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, 0.001f);
d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, 0.0001f);
d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, 0.0001f);
d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0.0001f);
d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0.0001f);
d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.0001f);
}
if (vel.Z < -30)
{
vel.Z = -30;
}
bool disabled = false;
if (_parent_scene.m_DisableSlowPrims)
{
if (((float)fz == (float)(_parent_scene.gravityz * m_mass)) &&
(vel.X < 0.01 || vel.Y < 0.01 || vel.Z < 0.0001) &&
m_forcelist.Count == 0)
{
if (Math.Abs(vel.X) < 0.0001 || Math.Abs(vel.Y) < 0.0001 || Math.Abs(vel.Z) < 0.0001)
{
Vector3 angvelocity = new Vector3((float)angvel.X, (float)angvel.Y, (float)angvel.Z);
if (Math.Abs(force.X) < 100 && Math.Abs(force.Y) < 100 && Math.Abs(force.Z) < 100 &&
angvelocity.ApproxEquals(Vector3.Zero, 0.01f) &&
vel.X != 0 && vel.Y != 0 && vel.Z != 0)
{
if (d.BodyIsEnabled(Body))
{
d.BodySetLinearVel(Body, 0, 0, 0);
d.BodySetForce(Body, 0, 0, 0);
d.BodyDisable(Body);
disabled = true;
}
}
else
{
if (!d.BodyIsEnabled(Body))
d.BodyEnable(Body);
}
}
else
{
if (!d.BodyIsEnabled(Body))
{
d.BodyEnable(Body);
fz = 100 * m_mass;
}
}
}
}
if (!disabled)
{
if (!d.BodyIsEnabled(Body))
{
// A physical body at rest on a surface will auto-disable after a while,
// this appears to re-enable it incase the surface it is upon vanishes,
// and the body should fall again.
d.BodySetLinearVel(Body, 0f, 0f, m_mass);
d.BodySetForce(Body, 0, 0, 100 * m_mass);
enableBodySoft();
vel = d.BodyGetLinearVel(Body);
}
float Drag = Mass / 2;
if (fx != 0)
fx -= Drag;
if (fy != 0)
fy -= Drag;
if (fz < 0)
fz += Drag;
else
fz -= Drag;
#region Force List
Vector3 iforce = Vector3.Zero;
int i = 0;
try
{
for (i = 0; i < m_forcelist.Count; i++)
{
iforce = iforce + (m_forcelist[i] * 100);
}
}
catch (IndexOutOfRangeException)
{
m_forcelist = new List<Vector3>();
m_collisionscore = 0;
m_interpenetrationcount = 0;
m_taintforce = false;
return;
}
catch (ArgumentOutOfRangeException)
{
m_forcelist = new List<Vector3>();
m_collisionscore = 0;
m_interpenetrationcount = 0;
m_taintforce = false;
return;
}
fx += iforce.X;
fy += iforce.Y;
fz += iforce.Z;
m_forcelist.Clear();
#endregion
d.BodyAddForce(Body, fx, fy, fz);
}
}
}
}
else
{ // is not physical, or is not a body or is selected
// _zeroPosition = d.BodyGetPosition(Body);
return;
//Console.WriteLine("Nothing " + m_primName);
}
}
/// <summary>
/// Thread to update listener position and generally keep
/// FMOD up to date.
/// </summary>
private void ListenerUpdate()
{
// Notice changes in position or direction.
Vector3 lastpos = new Vector3(0.0f, 0.0f, 0.0f);
float lastface = 0.0f;
while (true)
{
// Two updates per second.
Thread.Sleep(500);
if (system == null) continue;
AgentManager my = Instance.Client.Self;
Vector3 newPosition = new Vector3(my.SimPosition);
float newFace = my.SimRotation.W;
// If we are standing still, nothing to update now, but
// FMOD needs a 'tick' anyway for callbacks, etc. In looping
// 'game' programs, the loop is the 'tick'. Since Radegast
// uses events and has no loop, we use this position update
// thread to drive the FMOD tick. Have to move more than
// 500mm or turn more than 10 desgrees to bother with.
//
if (newPosition.ApproxEquals(lastpos, 0.5f) &&
Math.Abs(newFace - lastface) < 0.2)
{
invoke(new SoundDelegate(delegate
{
FMODExec(system.update());
}));
continue;
}
// We have moved or turned. Remember new position.
lastpos = newPosition;
lastface = newFace;
// Convert coordinate spaces.
FMOD.VECTOR listenerpos = FromOMVSpace(newPosition);
// Get azimuth from the facing Quaternion. Note we assume the
// avatar is standing upright. Avatars in unusual positions
// hear things from unpredictable directions.
// By definition, facing.W = Cos( angle/2 )
// With angle=0 meaning East.
double angle = 2.0 * Math.Acos(newFace);
// Construct facing unit vector in FMOD coordinates.
// Z is East, X is South, Y is up.
FMOD.VECTOR forward = new FMOD.VECTOR();
forward.x = (float)Math.Sin(angle); // South
forward.y = 0.0f;
forward.z = (float)Math.Cos(angle); // East
//Logger.Log(
// String.Format(
// "Standing at <{0:0.0},{1:0.0},{2:0.0}> facing {3:d}",
// listenerpos.x,
// listenerpos.y,
// listenerpos.z,
// (int)(angle * 180.0 / 3.141592)),
// Helpers.LogLevel.Debug);
// Tell FMOD the new orientation.
invoke(new SoundDelegate(delegate
{
FMODExec(system.set3DListenerAttributes(
0,
ref listenerpos, // Position
ref ZeroVector, // Velocity
ref forward, // Facing direction
ref UpVector)); // Top of head
FMODExec(system.update());
}));
}
}
internal void Move(float timestep)
{
//TODO:
float fx = 0;
float fy = 0;
float fz = 0;
if (IsPhysical && Body != null && Body.Handle != IntPtr.Zero && !m_isSelected)
{
float m_mass = CalculateMass();
fz = 0f;
//m_log.Info(m_collisionFlags.ToString());
if (m_buoyancy != 0)
{
if (m_buoyancy > 0)
{
fz = (((-1 * _parent_scene.gravityz) * m_buoyancy) * m_mass) * 0.035f;
//d.Vector3 l_velocity = d.BodyGetLinearVel(Body);
//m_log.Info("Using Buoyancy: " + buoyancy + " G: " + (_parent_scene.gravityz * m_buoyancy) + "mass:" + m_mass + " Pos: " + Position.ToString());
}
else
{
fz = (-1 * (((-1 * _parent_scene.gravityz) * (-1 * m_buoyancy)) * m_mass) * 0.035f);
}
}
if (m_usePID)
{
PID_D = 61f;
PID_G = 65f;
//if (!d.BodyIsEnabled(Body))
//d.BodySetForce(Body, 0f, 0f, 0f);
// If we're using the PID controller, then we have no gravity
fz = ((-1 * _parent_scene.gravityz) * m_mass) * 1.025f;
// 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) && (m_PIDTau != 0))
{
//PID_G = PID_G / m_PIDTau;
m_PIDTau = 1;
}
if ((PID_G - m_PIDTau) <= 0)
{
PID_G = m_PIDTau + 1;
}
// TODO: NEED btVector3 for Linear Velocity
// NEED btVector3 for Position
Vector3 pos = _position; //TODO: Insert values gotten from bullet
Vector3 vel = _velocity;
_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 (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
{
/* TODO: Do Bullet equiv
*
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;
fx = ((_target_velocity.X) - vel.X) * (PID_D);
fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
}
}
if (m_useHoverPID && !m_usePID)
{
// If we're using the PID controller, then we have no gravity
fz = (-1 * _parent_scene.gravityz) * m_mass;
// 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;
}
Vector3 pos = Vector3.Zero; //TODO: Insert values gotten from bullet
Vector3 vel = Vector3.Zero;
// determine what our target height really is based on HoverType
switch (m_PIDHoverType)
{
case PIDHoverType.Absolute:
m_targetHoverHeight = m_PIDHoverHeight;
break;
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)
{
m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
}
else
{
m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
}
break;
case PIDHoverType.Water:
m_waterHeight = _parent_scene.GetWaterLevel();
m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
break;
}
_target_velocity =
new Vector3(0.0f, 0.0f,
(m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
);
// if velocity is zero, use position control; otherwise, velocity control
if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
{
/* TODO: Do Bullet Equiv
d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
d.BodyAddForce(Body, 0, 0, fz);
*/
if (Body != null && Body.Handle != IntPtr.Zero)
{
Body.setLinearVelocity(_parent_scene.VectorZero);
Body.clearForces();
}
return;
}
else
{
_zeroFlag = false;
// We're flying and colliding with something
fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
}
}
fx *= m_mass;
fy *= m_mass;
//fz *= m_mass;
fx += m_force.X;
fy += m_force.Y;
fz += m_force.Z;
//m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
if (fx != 0 || fy != 0 || fz != 0)
{
/*
* TODO: Do Bullet Equiv
if (!d.BodyIsEnabled(Body))
{
d.BodySetLinearVel(Body, 0f, 0f, 0f);
d.BodySetForce(Body, 0, 0, 0);
enableBodySoft();
}
*/
// 35x10 = 350n times the mass per second applied maximum.
float nmax = 35f * m_mass;
float nmin = -35f * m_mass;
if (fx > nmax)
fx = nmax;
if (fx < nmin)
fx = nmin;
if (fy > nmax)
fy = nmax;
if (fy < nmin)
fy = nmin;
// TODO: Do Bullet Equiv
// d.BodyAddForce(Body, fx, fy, fz);
if (Body != null && Body.Handle != IntPtr.Zero)
{
Body.activate(true);
if (tempAddForce != null && tempAddForce.Handle != IntPtr.Zero)
tempAddForce.Dispose();
tempAddForce = new btVector3(fx * 0.01f, fy * 0.01f, fz * 0.01f);
Body.applyCentralImpulse(tempAddForce);
}
}
}
else
{
if (m_zeroPosition == null)
m_zeroPosition = Vector3.Zero;
m_zeroPosition = _position;
return;
}
}
// The physics engine says that properties have updated. Update same and inform
// the world that things have changed.
public void UpdateProperties(EntityProperties entprop)
{
bool changed = false;
#region Updating Position
if (entprop.ID != 0)
{
// we assign to the local variables so the normal set action does not happen
if (_position != entprop.Position)
{
_position = entprop.Position;
changed = true;
}
if (_orientation != entprop.Rotation)
{
_orientation = entprop.Rotation;
changed = true;
}
if (_velocity != entprop.Velocity)
{
changed = true;
_velocity = entprop.Velocity;
}
if (_acceleration != entprop.Acceleration)
{
_acceleration = entprop.Acceleration;
changed = true;
}
if (_rotationalVelocity != entprop.RotationalVelocity)
{
changed = true;
_rotationalVelocity = entprop.RotationalVelocity;
}
if (changed)
TriggerMovementUpdate();
}
#endregion
#region Jump code
if (_preJumping && Util.EnvironmentTickCountSubtract(_preJumpTime) > _scene.PreJumpTime)
{
//Time to jump
_jumping = true;
_preJumping = false;
Velocity += _preJumpForce;
_targetVelocityIsDecaying = false;
TriggerMovementUpdate();
}
if (_jumping && Util.EnvironmentTickCountSubtract(_preJumpTime) > _scene.PreJumpTime + 2000)
{
_jumping = false;
_targetVelocity = Vector3.Zero;
TriggerMovementUpdate();
}
else if (_jumping && Util.EnvironmentTickCountSubtract(_preJumpTime) > _scene.PreJumpTime + 750)
{
_targetVelocityIsDecaying = false;
TriggerMovementUpdate();
}
else if (_jumping && Util.EnvironmentTickCountSubtract(_preJumpTime) > _scene.PreJumpTime + 500)
{
_targetVelocityIsDecaying = false;
Velocity -= _preJumpForce/100; //Cut down on going up
TriggerMovementUpdate();
}
#endregion
#region Decaying velocity
if (_targetVelocityIsDecaying)
{
_targetVelocity *= _scene.DelayingVelocityMultiplier;
if (_targetVelocity.ApproxEquals(Vector3.Zero, 0.1f) || _velocity == Vector3.Zero)
_targetVelocity = Vector3.Zero;
}
if (_targetVelocity != Vector3.Zero)
Velocity = new Vector3(
_targetVelocity.X == 0
? Velocity.X
: (_targetVelocity.X*0.25f) + (Velocity.X*0.75f),
_targetVelocity.Y == 0
? Velocity.Y
: (_targetVelocity.Y*0.25f) + (Velocity.Y*0.75f),
_targetVelocity.Z == 0
? Velocity.Z
: (_targetVelocity.Z*0.25f) + (Velocity.Z*0.75f));
else if (Velocity.X != 0 || Velocity.Y != 0 || Velocity.Z > 0)
Velocity *= _scene.DelayingVelocityMultiplier;
if (Velocity.ApproxEquals(Vector3.Zero, 0.3f))
Velocity = Vector3.Zero;
#endregion
}
/// <summary>
/// Called from Simulate
/// This is the avatar's movement control + PID Controller
/// </summary>
/// <param name = "timeStep"></param>
public void Move(float timeStep, ref List<AuroraODECharacter> defects)
{
// 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 (Body == IntPtr.Zero)
return;
if (!m_shouldBePhysical)
return;
Vector3 vec = Vector3.Zero;
d.Vector3 vel = d.BodyGetLinearVel(Body);
d.Vector3 tempPos;
d.BodyCopyPosition(Body, out tempPos);
#region Flight Ceiling
// rex, added height check
if (m_pidControllerActive == false)
{
_zeroPosition = tempPos;
}
if (_parent_scene.m_useFlightCeilingHeight && tempPos.Z > _parent_scene.m_flightCeilingHeight)
{
tempPos.Z = _parent_scene.m_flightCeilingHeight;
d.BodySetPosition(Body, tempPos.X, tempPos.Y, tempPos.Z);
if (vel.Z > 0.0f)
{
vel.Z = 0.0f;
d.BodySetLinearVel(Body, vel.X, vel.Y, vel.Z);
}
if (_target_velocity.Z > 0.0f)
_target_velocity.Z = 0.0f;
}
// endrex
#endregion
#region NonFinite Pos
Vector3 localPos = new Vector3(tempPos.X, tempPos.Y, tempPos.Z);
if (!IsFinite(localPos))
{
MainConsole.Instance.Warn("[PHYSICS]: Avatar Position is non-finite!");
defects.Add(this);
// _parent_scene.RemoveCharacter(this);
// destroy avatar capsule and related ODE data
if (Amotor != IntPtr.Zero)
{
// Kill the Amotor
d.JointDestroy(Amotor);
Amotor = IntPtr.Zero;
}
//kill the Geometry
_parent_scene.waitForSpaceUnlock(_parent_scene.space);
if (Body != IntPtr.Zero)
{
//kill the body
d.BodyDestroy(Body);
Body = IntPtr.Zero;
}
if (Shell != IntPtr.Zero)
{
d.GeomDestroy(Shell);
Shell = IntPtr.Zero;
}
return;
}
#endregion
#region Check for out of region
if (Position.X < 0.25f || Position.Y < 0.25f ||
Position.X > _parent_scene.Region.RegionSizeX - .25f ||
Position.Y > _parent_scene.Region.RegionSizeY - .25f)
{
if (!CheckForRegionCrossing())
{
Vector3 newPos = Position;
newPos.X = Util.Clip(Position.X, 0.75f, _parent_scene.Region.RegionSizeX - 0.75f);
newPos.Y = Util.Clip(Position.Y, 0.75f, _parent_scene.Region.RegionSizeY - 0.75f);
Position = newPos;
d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z);
}
}
#endregion
#region Movement Multiplier
float movementmult = 1f;
if (!m_alwaysRun)
movementmult /= _parent_scene.avMovementDivisorWalk;
else
movementmult /= _parent_scene.avMovementDivisorRun;
movementmult *= 10;
movementmult *= SpeedModifier;
// movementmult *= 1 / _parent_scene.TimeDilation;
if (flying)
movementmult *= _parent_scene.m_AvFlySpeed;
#endregion
#region Check for underground
d.AABB aabb;
d.GeomGetAABB(Shell, out aabb);
float chrminZ = aabb.MinZ;
Vector3 posch = localPos;
float ftmp;
if (flying)
{
ftmp = 0.75f*timeStep;
posch.X += vel.X*ftmp;
posch.Y += vel.Y*ftmp;
}
float groundHeight = _parent_scene.GetTerrainHeightAtXY(posch.X, posch.Y);
if (chrminZ < groundHeight)
{
float depth = groundHeight - chrminZ;
if (_target_velocity.Z < 0)
_target_velocity.Z = 0;
if (!flying)
{
if (vel.Z < -10f)
vel.Z = -10f;
vec.Z = -vel.Z*PID_D*1.5f + depth*PID_P*50.0f;
}
else
{
vec.Z = depth*PID_P*50.0f;
}
if (depth < 0.12f)
{
m_iscolliding = true;
m_colliderfilter = 2;
m_iscollidingGround = true;
ContactPoint point = new ContactPoint
{
Type = ActorTypes.Ground,
PenetrationDepth = depth,
Position = {X = localPos.X, Y = localPos.Y, Z = chrminZ},
SurfaceNormal = new Vector3(0, 0, -1f)
};
//0 is the ground localID
AddCollisionEvent(0, point);
vec.Z *= 0.5f;
}
else
m_iscollidingGround = false;
}
else
m_iscollidingGround = false;
/*
if(Flying && _target_velocity == Vector3.Zero &&
Math.Abs(vel.Z) < 0.1)
notMoving = true;
*/
#endregion
#region Movement
#region Jump code
if (IsJumping)
{
// if ((IsColliding) && m_preJumpCounter > _parent_scene.m_preJumpTime || m_preJumpCounter > 150)
if ((IsColliding) && m_preJumpCounter > _parent_scene.m_preJumpTime || m_preJumpCounter > 150)
{
m_isJumping = false;
m_preJumpCounter = 0;
_target_velocity.Z = 0;
}
else
m_preJumpCounter++;
}
else if (m_ispreJumping)
{
if (m_preJumpCounter == _parent_scene.m_preJumpTime)
{
m_ispreJumping = false;
_target_velocity.X = m_preJumpForce.X*_parent_scene.m_preJumpForceMultiplierX;
_target_velocity.Y = m_preJumpForce.Y*_parent_scene.m_preJumpForceMultiplierY;
_target_velocity.Z = m_preJumpForce.Z*_parent_scene.m_preJumpForceMultiplierZ;
m_preJumpCounter = 0;
m_isJumping = true;
}
else
{
m_preJumpCounter++;
TriggerMovementUpdate();
return;
}
}
//This is for jumping on prims, since otherwise, you don't get off the ground sometimes
// if (m_iscolliding && m_isJumping && _target_velocity.Z < 1 && !Flying)
// _target_velocity.Z += m_preJumpForce.Z * _parent_scene.m_preJumpForceMultiplier;
#endregion
Vector3 gravForce = new Vector3();
// if velocity is zero, use position control; otherwise, velocity control
if (_target_velocity == Vector3.Zero &&
Math.Abs(vel.X) < 0.1 && Math.Abs(vel.Y) < 0.1 && Math.Abs(vel.Z) < 0.1 &&
(this.m_iscolliding || this.flying || (this._zeroFlag && _wasZeroFlagFlying == flying)))
//This is so that if we get moved by something else, it will update us in the client
{
m_isJumping = false;
// keep track of where we stopped. No more slippin' & slidin'
if (!_zeroFlag)
{
_zeroFlag = true;
_wasZeroFlagFlying = flying;
_zeroPosition = tempPos;
}
if (m_pidControllerActive)
{
// 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
// if target vel is zero why was it here ?
vec.X = -vel.X*PID_D + (_zeroPosition.X - tempPos.X)*PID_P;
vec.Y = -vel.Y*PID_D + (_zeroPosition.Y - tempPos.Y)*PID_P;
// if (!realFlying)
// vec.Z += - vel.Z * PID_D * 5;
// else
if (flying)
vec.Z += -vel.Z*PID_D*0.5f + (_zeroPosition.Z - tempPos.Z)*PID_P;
// _parent_scene.CalculateGravity(m_mass, tempPos, true, 0.15f, ref gravForce);
// vec += gravForce;
}
}
else
{
m_pidControllerActive = true;
_zeroFlag = false;
if (m_iscolliding)
{
if (!flying) //If there is a ground collision, it sets flying to false, so check against real flying
{
// We're standing or walking on something
if (_target_velocity.X != 0.0f)
vec.X += (_target_velocity.X * movementmult - vel.X) * PID_D * 2;
if (_target_velocity.Y != 0.0f)
vec.Y += (_target_velocity.Y * movementmult - vel.Y) * PID_D * 2;
if (_target_velocity.Z != 0.0f)
vec.Z += (_target_velocity.Z * movementmult - vel.Z) * PID_D;
/*// We're standing or walking on something
vec.X += (_target_velocity.X*movementmult - vel.X)*PID_D*2;
vec.Y += (_target_velocity.Y*movementmult - vel.Y)*PID_D*2;
if (_target_velocity.Z > 0.0f)
vec.Z += (_target_velocity.Z*movementmult - vel.Z)*PID_D;
// + (_zeroPosition.Z - tempPos.Z) * PID_P)) _zeropos maybe bad here*/
}
else
{
// We're flying and colliding with something
vec.X += (_target_velocity.X*movementmult - vel.X)*PID_D*0.5f;
vec.Y += (_target_velocity.Y*movementmult - vel.Y)*PID_D*0.5f;
//if(_target_velocity.Z > 0)
vec.Z += (_target_velocity.Z*movementmult - vel.Z)*PID_D*0.5f;
}
}
else
{
if (flying)
{
// we're flying
vec.X += (_target_velocity.X * movementmult - vel.X) * PID_D * 0.75f;
vec.Y += (_target_velocity.Y * movementmult - vel.Y) * PID_D * 0.75f;
}
else
{
// we're not colliding and we're not flying so that means we're falling!
// m_iscolliding includes collisions with the ground.
vec.X += (_target_velocity.X - vel.X) * PID_D * 0.85f;
vec.Y += (_target_velocity.Y - vel.Y) * PID_D * 0.85f;
}
}
if (flying)
{
#region Av gravity
if (_parent_scene.AllowAvGravity &&
tempPos.Z > _parent_scene.AvGravityHeight) //Should be stop avies from flying upwards
{
//Decay going up
if (_target_velocity.Z > 0)
{
//How much should we force them down?
float Multiplier = (_parent_scene.AllowAvsToEscapeGravity ? .03f : .1f);
//How much should we force them down?
float fudgeHeight = (_parent_scene.AllowAvsToEscapeGravity ? 80 : 30);
//We add the 30 so that gravity is resonably strong once they pass the min height
Multiplier *= tempPos.Z + fudgeHeight - _parent_scene.AvGravityHeight;
//Limit these so that things don't go wrong
if (Multiplier < 1)
Multiplier = 1;
float maxpower = (_parent_scene.AllowAvsToEscapeGravity ? 1.5f : 3f);
if (Multiplier > maxpower)
Multiplier = maxpower;
_target_velocity.Z /= Multiplier;
vel.Z /= Multiplier;
}
}
#endregion
vec.Z = (_target_velocity.Z*movementmult - vel.Z)*PID_D*0.5f;
if (_parent_scene.AllowAvGravity && tempPos.Z > _parent_scene.AvGravityHeight)
//Add extra gravity
vec.Z += ((10*_parent_scene.gravityz)*Mass);
}
}
if (realFlying)
{
#region Auto Fly Height
//Added for auto fly height. Kitto Flora
//Changed to only check if the avatar is flying around,
// Revolution: If the avatar is going down, they are trying to land (probably), so don't push them up to make it harder
// Only if they are moving around sideways do we need to push them up
if (_target_velocity.X != 0 || _target_velocity.Y != 0)
{
Vector3 forwardVel = new Vector3(_target_velocity.X > 0 ? 2 : (_target_velocity.X < 0 ? -2 : 0),
_target_velocity.Y > 0 ? 2 : (_target_velocity.Y < 0 ? -2 : 0),
0);
float target_altitude = _parent_scene.GetTerrainHeightAtXY(tempPos.X, tempPos.Y) +
MinimumGroundFlightOffset;
//We cheat a bit and do a bit lower than normal
if ((tempPos.Z - CAPSULE_LENGTH) < target_altitude ||
(tempPos.Z - CAPSULE_LENGTH) <
_parent_scene.GetTerrainHeightAtXY(tempPos.X + forwardVel.X, tempPos.Y + forwardVel.Y)
+ MinimumGroundFlightOffset)
if (_target_velocity.Z < 0)
vec.Z += (target_altitude - tempPos.Z)*PID_P*0.5f; //Don't apply so much
else
vec.Z += (target_altitude - tempPos.Z)*PID_P*1.05f;
}
else
{
//Straight up and down, only apply when they are very close to the ground
float target_altitude = _parent_scene.GetTerrainHeightAtXY(tempPos.X, tempPos.Y);
if ((tempPos.Z - CAPSULE_LENGTH + (MinimumGroundFlightOffset/1.5)) <
target_altitude + MinimumGroundFlightOffset)
{
if ((tempPos.Z - CAPSULE_LENGTH) < target_altitude + 1)
{
vec.Z += ((target_altitude + 4) - (tempPos.Z - CAPSULE_LENGTH))*PID_P;
}
else
vec.Z += ((target_altitude + MinimumGroundFlightOffset) - (tempPos.Z - CAPSULE_LENGTH))*
PID_P*0.5f;
}
}
#endregion
}
#region Gravity
if (!flying)
_parent_scene.CalculateGravity(m_mass, tempPos, true, 1.0f, ref gravForce);
else
_parent_scene.CalculateGravity(m_mass, tempPos, false, 0.65f, ref gravForce);
//Allow point gravity and repulsors affect us a bit
vec += gravForce;
#endregion
#region Under water physics
if (_parent_scene.AllowUnderwaterPhysics && tempPos.X < _parent_scene.Region.RegionSizeX &&
tempPos.Y < _parent_scene.Region.RegionSizeY)
{
//Position plus height to av's shoulder (aprox) is just above water
if ((tempPos.Z + (CAPSULE_LENGTH/3) - .25f) < _parent_scene.GetWaterLevel(tempPos.X, tempPos.Y))
{
if (StartingUnderWater)
ShouldBeWalking = Flying = false;
StartingUnderWater = false;
WasUnderWater = true;
Flying = true;
lastUnderwaterPush = 0;
if (ShouldBeWalking)
{
lastUnderwaterPush += (float) (_parent_scene.GetWaterLevel(tempPos.X, tempPos.Y) - tempPos.Z)*33 +
3;
vec.Z += lastUnderwaterPush;
}
else
{
lastUnderwaterPush += 3500;
lastUnderwaterPush += (float) (_parent_scene.GetWaterLevel(tempPos.X, tempPos.Y) - tempPos.Z)*8;
vec.Z += lastUnderwaterPush;
}
}
else
{
StartingUnderWater = true;
if (WasUnderWater)
{
WasUnderWater = false;
Flying = true;
}
}
}
#endregion
#endregion
#region Apply the force
if (IsFinite(vec))
{
if (vec.X < 100000000 && vec.Y < 10000000 && vec.Z < 10000000)
//Checks for crazy, going to NaN us values
{
// round small values to zero. those possible are just errors
if (Math.Abs(vec.X) < 0.001)
vec.X = 0;
if (Math.Abs(vec.Y) < 0.001)
vec.Y = 0;
if (Math.Abs(vec.Z) < 0.001)
vec.Z = 0;
//ODE autodisables not moving prims, accept it and reenable when we need to
if (!d.BodyIsEnabled(Body))
d.BodyEnable(Body);
if (vec == Vector3.Zero) //if we arn't moving, STOP
{
if (m_lastForceApplied != -1)
{
m_lastForceApplied = -1;
d.BodySetLinearVel(Body, vec.X, vec.Y, vec.Z);
}
}
else
{
if (m_lastForceApplied < 5)
vec *= m_lastForceApplied / 5;
doForce(vec);
m_lastForceApplied++;
}
// if (!_zeroFlag && (!flying || m_iscolliding))
// AlignAvatarTiltWithCurrentDirectionOfMovement (vec, gravForce);
// the Amotor still lets avatar rotation to drift during colisions
// so force it back to identity
d.Quaternion qtmp;
qtmp.W = 1;
qtmp.X = 0;
qtmp.Y = 0;
qtmp.Z = 0;
d.BodySetQuaternion(Body, ref qtmp);
d.BodySetAngularVel(Body, 0, 0, 0);
//When falling, we keep going faster and faster, and eventually, the client blue screens (blue is all you see).
// The speed that does this is slightly higher than -30, so we cap it here so we never do that during falling.
if (vel.Z < -30)
{
vel.Z = -30;
d.BodySetLinearVel(Body, vel.X, vel.Y, vel.Z);
}
//Decay out the target velocity DON'T it forces tons of updates
_target_velocity *= _parent_scene.m_avDecayTime;
if (!_zeroFlag && _target_velocity.ApproxEquals (Vector3.Zero, _parent_scene.m_avStopDecaying))
_target_velocity = Vector3.Zero;
}
else
{
//This is a safe guard from going NaN, but it isn't very smooth... which is ok
d.BodySetForce(Body, 0, 0, 0);
d.BodySetLinearVel(Body, 0, 0, 0);
}
}
else
{
MainConsole.Instance.Warn("[PHYSICS]: Got a NaN force vector in Move()");
MainConsole.Instance.Warn("[PHYSICS]: Avatar Position is non-finite!");
defects.Add(this);
//kill the Geometry
_parent_scene.waitForSpaceUnlock(_parent_scene.space);
if (Body != IntPtr.Zero)
{
//kill the body
d.BodyDestroy(Body);
Body = IntPtr.Zero;
}
if (Shell != IntPtr.Zero)
{
d.GeomDestroy(Shell);
Shell = IntPtr.Zero;
}
}
#endregion
}
public void Move(float timestep)
{
float fx = 0;
float fy = 0;
float fz = 0;
if (IsPhysical && Body != IntPtr.Zero && !m_isSelected)
{
//float PID_P = 900.0f;
float m_mass = CalculateMass();
fz = 0f;
//m_log.Info(m_collisionFlags.ToString());
if (m_buoyancy != 0)
{
if (m_buoyancy > 0)
{
fz = (((-1 * _parent_scene.gravityz) * m_buoyancy) * m_mass);
//d.Vector3 l_velocity = d.BodyGetLinearVel(Body);
//m_log.Info("Using Buoyancy: " + buoyancy + " G: " + (_parent_scene.gravityz * m_buoyancy) + "mass:" + m_mass + " Pos: " + Position.ToString());
}
else
{
fz = (-1 * (((-1 * _parent_scene.gravityz) * (-1 * m_buoyancy)) * m_mass));
}
}
if (m_usePID)
{
// If we're using the PID controller, then we have no gravity
fz = (-1 * _parent_scene.gravityz) * m_mass;
// 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;
}
//PidStatus = true;
// PhysicsVector vec = new PhysicsVector();
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 = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
}
}
fx *= m_mass;
fy *= m_mass;
//fz *= m_mass;
fx += m_force.X;
fy += m_force.Y;
fz += m_force.Z;
//m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
if (fx != 0 || fy != 0 || fz != 0)
{
//m_taintdisable = true;
//base.RaiseOutOfBounds(Position);
//d.BodySetLinearVel(Body, fx, fy, 0f);
if (!d.BodyIsEnabled(Body))
{
d.BodySetLinearVel(Body, 0f, 0f, 0f);
d.BodySetForce(Body, 0, 0, 0);
enableBodySoft();
}
d.BodyAddForce(Body, fx, fy, fz);
}
}
else
{
// _zeroPosition = d.BodyGetPosition(Body);
return;
}
}
} // end MoveLinear()
private void MoveAngular (float pTimestep, AuroraODEPhysicsScene _pParentScene, AuroraODEPrim parent)
{
d.Vector3 angularVelocity = d.BodyGetAngularVel (Body);
d.Quaternion rot = d.BodyGetQuaternion (Body);
Quaternion rotq = new Quaternion (rot.X, rot.Y, rot.Z, rot.W);
// Vector3 angularVelocity = Vector3.Zero;
/*if ((m_flags & VehicleFlag.MOUSELOOK_STEER) == VehicleFlag.MOUSELOOK_STEER)
{
if (m_userLookAt != Quaternion.Identity)
{
Quaternion camrot = Quaternion.Subtract (m_userLookAt, rotq);
camrot.Normalize ();
m_angularMotorVelocity += Vector3.One * camrot;
Console.WriteLine (Vector3.One * camrot);
}
}*/
if (m_angularMotorApply > 90)
{
// ramp up to new value
// current velocity += error / (time to get there / step interval)
// requested speed - last motor speed
m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / (pTimestep / 2));
m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / (pTimestep / 2));
m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / (pTimestep / 2));
m_angularMotorApply--; // This is done so that if script request rate is less than phys frame rate the expected
// velocity may still be acheived.
m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / pTimestep);
}
else if(m_angularMotorVelocity != Vector3.Zero)
{
// no motor recently applied, keep the body velocity
/* m_angularMotorVelocity.X = angularVelocity.X;
m_angularMotorVelocity.Y = angularVelocity.Y;
m_angularMotorVelocity.Z = angularVelocity.Z; */
// and decay the velocity
m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / (pTimestep * 3));
if(m_angularMotorVelocity.ApproxEquals(Vector3.Zero, 0.1f))
m_angularMotorVelocity = Vector3.Zero;
} // end motor section
if(m_angularMotorApply > 0)
m_angularMotorApply--;
// Vertical attractor section
Vector3 vertattr = Vector3.Zero;
Vector3 deflection = Vector3.Zero;
Vector3 banking = Vector3.Zero;
if(m_verticalAttractionTimescale < 300 && (m_lastAngularVelocity != Vector3.Zero || m_angularMotorApply > 90))
{
float VAservo = 0;
if(Type == Vehicle.TYPE_BOAT)
{
VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep);
VAservo *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
}
else
{
if(parent.LinkSetIsColliding)
VAservo = 0.05f / (m_verticalAttractionTimescale * pTimestep);
else
VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep);
VAservo *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
}
// get present body rotation
// make a vector pointing up
Vector3 verterr = Vector3.Zero;
verterr.Z = 1.0f;
// rotate it to Body Angle
verterr = verterr * rotq;
// verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1.
// As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go
// negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
if(verterr.Z < 0.0f)
{
verterr.X = 2.0f - verterr.X;
verterr.Y = 2.0f - verterr.Y;
}
// Error is 0 (no error) to +/- 2 (max error)
// scale it by VAservo
verterr = verterr * VAservo;
//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
// Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
vertattr.X = verterr.Y;
vertattr.Y = -verterr.X;
vertattr.Z = 0f;
// scaling appears better usingsquare-law
float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
vertattr.X += bounce * angularVelocity.X;
vertattr.Y += bounce * angularVelocity.Y;
} // else vertical attractor is off
#region Deflection
//Forward is the prefered direction, but if the reference frame has changed, we need to take this into account as well
Vector3 PreferredAxisOfMotion = new Vector3 ((10 * (m_angularDeflectionEfficiency / m_angularDeflectionTimescale) * pTimestep), 0, 0);
PreferredAxisOfMotion *= Quaternion.Add(rotq, m_referenceFrame);
//Multiply it so that it scales linearly
//deflection = PreferredAxisOfMotion;
//deflection = ((PreferredAxisOfMotion * m_angularDeflectionEfficiency) / (m_angularDeflectionTimescale / pTimestep));
#endregion
#region Banking
if (m_bankingEfficiency != 0)
{
Vector3 angularMotorVelocity = new Vector3 ();
if (m_angularMotorApply > 90)
{
// ramp up to new value
// current velocity += error / (time to get there / step interval)
// requested speed - last motor speed
angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep * pTimestep * 16f);
angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep * pTimestep * 4f);
angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep * pTimestep * 4f);
// velocity may still be acheived.
Vector3 dir = Vector3.One * rotq;
float mult = (m_bankingMix * m_bankingMix) * -1 * (m_bankingMix < 0 ? -1 : 1);//Changes which way it banks in and out of turns
//Use the square of the efficiency, as it looks much more how SL banking works
float effSquared = (m_bankingEfficiency * m_bankingEfficiency);
if (m_bankingEfficiency < 0)
effSquared *= -1;//Keep the negative!
float mix = Math.Abs(m_bankingMix);
banking.Z += (effSquared * (mult * mix)) * (angularMotorVelocity.X);
m_angularMotorVelocity.X *= 1 - m_bankingEfficiency;
if(!parent.LinkSetIsColliding/* && Math.Abs(angularMotorVelocity.X) > mix*/) //If they are colliding, we probably shouldn't shove the prim around... probably
{
float angVelZ = angularMotorVelocity.X * -1;
/*if(angVelZ > mix)
angVelZ = mix;
else if(angVelZ < -mix)
angVelZ = -mix;*/
//This controls how fast and how far the banking occurs
Vector3 bankingRot = new Vector3(angVelZ * (effSquared * mult), 0, 0);
if(bankingRot.X > 3)
bankingRot.X = 3;
else if(bankingRot.X < -3)
bankingRot.X = -3;
bankingRot *= rotq;
banking += bankingRot;
}
}
}
#endregion
#region Downward Force
Vector3 downForce = Vector3.Zero;
double Zchange = m_lastposChange.Z;
if((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0) //if it isn't going up, don't apply the limiting force
{
if(Zchange < -0.1f)
{
if(Zchange < -0.3f)
Zchange = -0.3f;
//Requires idea of 'up', so use reference frame to rotate it
//Add to the X, because that will normally tilt the vehicle downward (if its rotated, it'll be rotated by the ref. frame
downForce = (new Vector3(0, ((float)Math.Abs(Zchange) * (pTimestep * _pParentScene.PID_P / 4)), 0));
downForce *= rotq;
}
}
#endregion
// Sum velocities
m_lastAngularVelocity = m_angularMotorVelocity + vertattr + deflection + banking + downForce;
if (!m_lastAngularVelocity.ApproxEquals (Vector3.Zero, 0.01f))
{
if (!d.BodyIsEnabled (Body))
d.BodyEnable (Body);
}
else
m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
#region Linear Motor Offset
//Offset section
if (m_linearMotorOffset != Vector3.Zero)
{
//Offset of linear velocity doesn't change the linear velocity,
// but causes a torque to be applied, for example...
//
// IIIII >>> IIIII
// IIIII >>> IIIII
// IIIII >>> IIIII
// ^
// | Applying a force at the arrow will cause the object to move forward, but also rotate
//
//
// The torque created is the linear velocity crossed with the offset
//Note: we use the motor, otherwise you will just spin around and we divide by 10 since otherwise we go crazy
Vector3 torqueFromOffset = (m_linearMotorDirectionLASTSET / m_linearMotorOffset);
if(float.IsNaN(torqueFromOffset.X))
torqueFromOffset.X = 0;
if(float.IsNaN(torqueFromOffset.Y))
torqueFromOffset.Y = 0;
if(float.IsNaN(torqueFromOffset.Z))
torqueFromOffset.Z = 0;
d.BodyAddTorque (Body, torqueFromOffset.X, torqueFromOffset.Y, torqueFromOffset.Z);
}
#endregion
/*if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
{
m_lastAngularVelocity.X = 0;
m_lastAngularVelocity.Y = 0;
}*/
// apply friction
Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep);
if(parent.LinkSetIsColliding)
{
decayamount *= 100;
if(decayamount.X > 1)
decayamount.X = 1;
if(decayamount.Y > 1)
decayamount.Y = 1;
if(decayamount.Z > 1)
decayamount.Z = 1;
}
m_lastAngularVelocity -= m_lastAngularVelocity * decayamount;
// Apply to the body
if(m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.1f))
{
m_lastAngularVelocity = Vector3.Zero;
d.BodySetAngularVel(Body, 0, 0, 0);
m_angularZeroFlag = true;
}
else
{
d.BodySetAngularVel(Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
m_angularZeroFlag = false;
}
}
} // end Step
private void MoveLinear (float pTimestep, AuroraODEPhysicsScene _pParentScene, AuroraODEPrim parent)
{
Vector3 motorDirection = m_linearMotorDirection;
if(!motorDirection.ApproxEquals(Vector3.Zero, 0.01f) || m_linearMotorApply > 90) // requested m_linearMotorDirection is significant
{
if(m_linearMotorApply <= 80)
if(m_linearMotorTimescale > 1)
m_linearMotorDirection /= m_linearMotorTimescale;
else
{
m_linearMotorDirection *= m_linearMotorTimescale;
motorDirection *= m_linearMotorTimescale;
}
if (!d.BodyIsEnabled (Body))
d.BodyEnable (Body);
//Interpolate between the current and last
float diff = 100 - m_linearMotorApply;
if(m_linearMotorApply >= 90)
motorDirection = (m_linearMotorDirection * (diff / 10f)) + (m_linearMotorDirectionLASTSET * (1 - (diff / 10f)));
// add drive to body
Vector3 addAmount = motorDirection / m_linearMotorTimescale;
addAmount *= pTimestep;
m_lastLinearVelocityVector += (addAmount); // lastLinearVelocityVector is the current body velocity vector?
//This is a huge problem with the Bwind script, it 'must' be disabled
// This will work temporarily, but we really need to compare speed on an axis
// KF: Limit body velocity to applied velocity?
/*if (Math.Abs (m_lastLinearVelocityVector.X) > Math.Abs (m_linearMotorDirectionLASTSET.X))
m_linearMotorDirection.X = m_linearMotorDirectionLASTSET.X;
if (Math.Abs (m_lastLinearVelocityVector.Y) > Math.Abs (m_linearMotorDirectionLASTSET.Y))
m_linearMotorDirection.Y = m_linearMotorDirectionLASTSET.Y;
if (Math.Abs (m_lastLinearVelocityVector.Z) > Math.Abs (m_linearMotorDirectionLASTSET.Z))
m_linearMotorDirection.Z = m_linearMotorDirectionLASTSET.Z;*/
if(!addAmount.ApproxEquals(Vector3.Zero, 0.01f))
{
// decay applied velocity
float decayTime = m_linearMotorDecayTimescale / pTimestep;
Vector3 decayfraction = Vector3.One / decayTime;
if(decayfraction.X > 0.9f)
decayfraction.X = 0.9f;
if(decayfraction.Y > 0.9f)
decayfraction.Y = 0.9f;
if(decayfraction.Z > 0.9f)
decayfraction.Z = 0.9f;
Vector3 decayAmt = (motorDirection * decayfraction);
motorDirection -= decayAmt;
decayAmt = (m_linearMotorDirection * decayfraction);
m_linearMotorDirection -= decayAmt;
}
if(m_linearMotorApply > 0)
m_linearMotorApply--;
}
else if(m_linearMotorApply > 0)
m_linearMotorApply--;
// convert requested object velocity to world-referenced vector
m_dir = m_lastLinearVelocityVector;
d.Quaternion rot = d.BodyGetQuaternion (Body);
Quaternion rotq = new Quaternion (rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
m_dir *= rotq; // apply obj rotation to velocity vector
// add Gravity andBuoyancy
// KF: So far I have found no good method to combine a script-requested
// .Z velocity and gravity. Therefore only 0g will used script-requested
// .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only.
Vector3 grav = Vector3.Zero;
// There is some gravity, make a gravity force vector
// that is applied after object velocity.
// m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
grav.Z = _pParentScene.gravityz * Mass * (float)parent.ParentEntity.GravityMultiplier * (1f - m_VehicleBuoyancy);
// Preserve the current Z velocity
d.Vector3 vel_now = d.BodyGetLinearVel (Body);
if (m_lastLinearVelocityVector.Z == 0 && (Type != Vehicle.TYPE_AIRPLANE && Type != Vehicle.TYPE_BALLOON))
m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity
//else if(Type != Vehicle.TYPE_AIRPLANE && Type != Vehicle.TYPE_BALLOON)
// m_dir.Z += vel_now.Z;
Vector3 pos = parent.Position;
// Vector3 accel = new Vector3(-(m_dir.X - m_lastLinearVelocityVector.X / 0.1f), -(m_dir.Y - m_lastLinearVelocityVector.Y / 0.1f), m_dir.Z - m_lastLinearVelocityVector.Z / 0.1f);
if(!(m_lastPositionVector.X == 0 &&
m_lastPositionVector.Y == 0 &&
m_lastPositionVector.Z == 0))
{
///Only do this if we have a last position
m_lastposChange.X = pos.X - m_lastPositionVector.X;
m_lastposChange.Y = pos.Y - m_lastPositionVector.Y;
m_lastposChange.Z = pos.Z - m_lastPositionVector.Z;
}
#region Blocking Change
double Zchange = Math.Abs(m_lastposChange.Z);
if (m_BlockingEndPoint != Vector3.Zero)
{
if (pos.X >= (m_BlockingEndPoint.X - (float)1))
{
pos.X -= m_lastposChange.X + 1;
d.BodySetPosition (Body, pos.X, pos.Y, pos.Z);
}
if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
{
pos.Y -= m_lastposChange.Y + 1;
d.BodySetPosition (Body, pos.X, pos.Y, pos.Z);
}
if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
{
pos.Z -= m_lastposChange.Z + 1;
d.BodySetPosition (Body, pos.X, pos.Y, pos.Z);
}
if (pos.X <= 0)
{
pos.X += m_lastposChange.X + 1;
d.BodySetPosition (Body, pos.X, pos.Y, pos.Z);
}
if (pos.Y <= 0)
{
pos.Y += m_lastposChange.Y + 1;
d.BodySetPosition (Body, pos.X, pos.Y, pos.Z);
}
}
#endregion
#region Terrain checks
float terrainHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
if(pos.Z < terrainHeight - 5)
{
pos.Z = terrainHeight + 2;
m_lastPositionVector = pos;//Make sure that we don't have an explosion the next frame with the posChange
d.BodySetPosition (Body, pos.X, pos.Y, pos.Z);
}
else if(pos.Z < terrainHeight)
{
m_dir.Z += 1;
}
#endregion
#region Hover
// Check if hovering
if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
{
// We should hover, get the target height
if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
{
m_VhoverTargetHeight = (float)_pParentScene.GetWaterLevel (pos.X, pos.Y) + m_VhoverHeight;
}
if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
{
m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY (pos.X, pos.Y) + m_VhoverHeight;
}
if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
{
m_VhoverTargetHeight = m_VhoverHeight;
}
float tempHoverHeight = m_VhoverTargetHeight;
if ((m_flags & VehicleFlag.HOVER_UP_ONLY) != 0)
{
// If body is aready heigher, use its height as target height
if(pos.Z > tempHoverHeight)
tempHoverHeight = pos.Z;
}
if ((m_flags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
{
if((pos.Z - tempHoverHeight) > .2 || (pos.Z - tempHoverHeight) < -.2)
{
float h = tempHoverHeight;
float groundHeight = _pParentScene.GetTerrainHeightAtXY (pos.X, pos.Y);
if(groundHeight >= tempHoverHeight)
h = groundHeight;
d.BodySetPosition(Body, pos.X, pos.Y, tempHoverHeight);
}
}
else
{
float herr0 = pos.Z - tempHoverHeight;
// Replace Vertical speed with correction figure if significant
if (herr0 > 0.01f)
{
m_dir.Z = -((herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
//KF: m_VhoverEfficiency is not yet implemented
}
else if(herr0 < -0.01f)
{
m_dir.Z = -((herr0 * pTimestep * 50f) / m_VhoverTimescale);
}
else
{
m_dir.Z = 0f;
}
}
// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
// m_VhoverTimescale = 0f; // time to acheive height
// pTimestep is time since last frame,in secs
}
#endregion
#region No X,Y,Z
if ((m_flags & (VehicleFlag.NO_X)) != 0)
m_dir.X = 0;
if ((m_flags & (VehicleFlag.NO_Y)) != 0)
m_dir.Y = 0;
if ((m_flags & (VehicleFlag.NO_Z)) != 0)
m_dir.Z = 0;
#endregion
#region Deal with tainted forces
// KF: So far I have found no good method to combine a script-requested
// .Z velocity and gravity. Therefore only 0g will used script-requested
// .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only.
// m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
Vector3 TaintedForce = new Vector3 ();
if (m_forcelist.Count != 0)
{
try
{
for (int i = 0; i < m_forcelist.Count; i++)
{
TaintedForce = TaintedForce + (m_forcelist[i]);
}
}
catch (IndexOutOfRangeException)
{
TaintedForce = Vector3.Zero;
}
catch (ArgumentOutOfRangeException)
{
TaintedForce = Vector3.Zero;
}
m_forcelist = new List<Vector3> ();
}
#endregion
#region Deflection
//Forward is the prefered direction
/*Vector3 deflectionamount = m_dir / (m_linearDeflectionTimescale / pTimestep);
//deflectionamount *= m_linearDeflectionEfficiency;
if (deflectionamount != Vector3.Zero)
{
}
Vector3 deflection = Vector3.One / deflectionamount;
m_dir /= deflection;*/
#endregion
#region limitations
if (Math.Abs (m_dir.X) > 1000 ||
Math.Abs (m_dir.Y) > 1000 ||
Math.Abs (m_dir.Z) > 1000)
{
m_dir = Vector3.Zero;
/*
//This vehicle is f***ed
parent.RaiseOutOfBounds (parent.Position);
parent._zeroFlag = true;
parent.m_disabled = true;
parent.m_frozen = true;*/
return;
}
#endregion
if(m_dir.ApproxEquals(Vector3.Zero, 0.001f))
m_dir = Vector3.Zero;
m_dir += TaintedForce;
m_lastPositionVector = parent.Position;
// Apply velocity
d.BodySetLinearVel (Body, m_dir.X, m_dir.Y, m_dir.Z);
// apply gravity force
d.BodyAddForce (Body, grav.X, grav.Y, grav.Z);
// apply friction
Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / (pTimestep));
/*if(parent.LinkSetIsColliding)
{
}*/
if(m_linearMotorDirectionLASTSET.X != 0 && (m_lastLinearVelocityVector.X / m_linearMotorDirectionLASTSET.X) < 10)
decayamount.X += 0.025f * (m_lastLinearVelocityVector.X / m_linearMotorDirectionLASTSET.X);
if(m_linearMotorDirectionLASTSET.Y != 0 && (m_lastLinearVelocityVector.Y / m_linearMotorDirectionLASTSET.Y) < 10)
decayamount.Y += 0.025f * (m_lastLinearVelocityVector.Y / m_linearMotorDirectionLASTSET.Y);
if(m_linearMotorDirectionLASTSET.Z != 0 && (m_lastLinearVelocityVector.Z / m_linearMotorDirectionLASTSET.Z) < 10)
decayamount.Z += 0.025f * (m_lastLinearVelocityVector.Z / m_linearMotorDirectionLASTSET.Z);
if(m_linearMotorApply <= 0)
decayamount += new Vector3(0.1f, 0.1f, 0.1f);
if(decayamount.X > 1)
decayamount.X = 1;
if(decayamount.Y > 1)
decayamount.Y = 1;
if(decayamount.Z > 1)
decayamount.Z = 1;
m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
if(m_linearMotorApply <= 0 ? m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.1f) :
m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.001f))
{
m_lastLinearVelocityVector = Vector3.Zero;
m_linearZeroFlag = true;
}
else
{
m_linearZeroFlag = false;
}
} // end MoveLinear()
public void Move(float timestep)
{
if (m_frozen)
return;
float fx = 0;
float fy = 0;
float fz = 0;
if (m_isphysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims.
{
if (m_vehicle.Type != Vehicle.TYPE_NONE)
{
// 'VEHICLES' are dealt with in ODEDynamics.cs
m_vehicle.Step(Body, timestep, _parent_scene, this);
d.Vector3 vel = d.BodyGetLinearVel(Body);
_velocity = new Vector3((float)vel.X, (float)vel.Y, (float)vel.Z);
d.Vector3 pos = d.GeomGetPosition(prim_geom);
_position = new Vector3((float)pos.X, (float)pos.Y, (float)pos.Z);
_zeroFlag = false;
}
else
{
float m_mass = _mass;
d.Vector3 dcpos = d.BodyGetPosition(Body);
d.Vector3 vel = d.BodyGetLinearVel(Body);
d.Vector3 angvel = d.BodyGetAngularVel(Body);
//KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
// would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ??
// m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
// gravityz multiplier = 1 - m_buoyancy
if (!_parent_scene.UsePointGravity)
{
if (!testRealGravity)
{
fx = _parent_scene.gravityx * (1.0f - m_buoyancy);
fy = _parent_scene.gravityy * (1.0f - m_buoyancy);
fz = _parent_scene.gravityz * (1.0f - m_buoyancy);
}
else
{
fx = _parent_scene.gravityx * -1 * (1.0f - m_buoyancy);
fy = _parent_scene.gravityy * -1 * (1.0f - m_buoyancy);
fz = _parent_scene.gravityz * -1 * (1.0f - m_buoyancy);
}
}
else
{
//Set up point gravity for this object
Vector3 cog = _parent_scene.PointOfGravity;
if (cog.X != 0)
fx = (cog.X - dcpos.X);
if (cog.Y != 0)
fy = (cog.Y - dcpos.Y);
if (cog.Z != 0)
fz = (cog.Z - dcpos.Z);
}
#region PID
if (m_usePID)
{
//Console.WriteLine("PID " + m_primName);
// KF - this is for object move? eg. llSetPos() ?
//if (!d.BodyIsEnabled(Body))
//d.BodySetForce(Body, 0f, 0f, 0f);
// If we're using the PID controller, then we have no gravity
//fz = (-1 * _parent_scene.gravityz) * m_mass; //KF: ?? Prims have no global gravity,so simply...
fz = 0f;
// 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) && (m_PIDTau != 0))
{
//PID_G = PID_G / m_PIDTau;
m_PIDTau = 1;
}
if ((PID_G - m_PIDTau) <= 0)
{
PID_G = m_PIDTau + 1;
}
//PidStatus = true;
// PhysicsVector vec = new PhysicsVector();
_target_velocity =
new Vector3(
(float)(m_PIDTarget.X - dcpos.X) * ((/*PID_G - */m_PIDTau) * timestep),
(float)(m_PIDTarget.Y - dcpos.Y) * ((/*PID_G - */m_PIDTau) * timestep),
(float)(m_PIDTarget.Z - dcpos.Z) * ((/*PID_G - */m_PIDTau) * timestep)
);
// if velocity is zero, use position control; otherwise, velocity control
if (_target_velocity.ApproxEquals(Vector3.Zero, 0.05f))
{
// 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);
if (!m_angularlock.ApproxEquals(Vector3.One, 0.003f) &&
Amotor != IntPtr.Zero)
{
}
d.BodySetLinearVel(Body, 0, 0, 0);
d.BodyAddForce(Body, 0, 0, fz);
return;
}
else
{
_zeroFlag = false;
// We're flying and colliding with something
fx = (float)((_target_velocity.X) - vel.X) * (PID_D);
fy = (float)((_target_velocity.Y) - vel.Y) * (PID_D);
// vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
fz = (float)(fz + ((_target_velocity.Z - vel.Z) * (PID_D)));
}
} // end if (m_usePID)
#endregion
#region Hover PID
// Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
if (m_useHoverPID && !m_usePID)
{
//Console.WriteLine("Hover " + m_primName);
// If we're using the PID controller, then we have no gravity
fx = -_parent_scene.gravityx;
fy = -_parent_scene.gravityy;
fz = -_parent_scene.gravityz;
// 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?
// 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((float)dcpos.X, (float)dcpos.Y);
m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
break;
case PIDHoverType.GroundAndWater:
m_groundHeight = _parent_scene.GetTerrainHeightAtXY((float)dcpos.X, (float)dcpos.Y);
m_waterHeight = (float)_parent_scene.GetWaterLevel((float)dcpos.X, (float)dcpos.Y);
if (m_groundHeight > m_waterHeight)
{
m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
}
else
{
m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
}
break;
} // end switch (m_PIDHoverType)
_target_velocity =
new Vector3(0.0f, 0.0f,
(float)(m_targetHoverHeight - dcpos.Z) * ((PID_G - m_PIDHoverTau) * 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
d.BodySetPosition(prim_geom, dcpos.X, dcpos.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 = (float)(fz + ((_target_velocity.Z - vel.Z) * (PID_D)));
}
}
#endregion
fx *= m_mass;
fy *= m_mass;
fz *= m_mass;
fx += m_force.X;
fy += m_force.Y;
fz += m_force.Z;
# region drag and forces accumulators
float drag = -m_mass * 0.2f;
fx += drag * vel.X;
fy += drag * vel.Y;
fz += drag * vel.Z;
Vector3 newtorque;
newtorque.X = m_angularforceacc.X;
newtorque.Y = m_angularforceacc.Y;
newtorque.Z = m_angularforceacc.Z;
m_angularforceacc = Vector3.Zero;
fx += m_forceacc.X;
fy += m_forceacc.Y;
fz += m_forceacc.Z;
m_forceacc = Vector3.Zero;
#endregion
//m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
if (fx != 0 || fy != 0 || fz != 0 || newtorque.X != 0 || newtorque.Y != 0 || newtorque.Z != 0)
{
// 35n times the mass per second applied maximum.
float nmax = 35f * m_mass;
float nmin = -35f * m_mass;
if (fx > nmax)
fx = nmax;
if (fx < nmin)
fx = nmin;
if (fy > nmax)
fy = nmax;
if (fy < nmin)
fy = nmin;
if (Amotor != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0.003f) )
{
d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0.001f);
d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0.0001f);
d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0.0001f);
d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0.0001f);
d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0.0001f);
d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0.0001f);
}
/*
if (vel.Z < -30)
{
vel.Z = -30;
}
*/
bool disabled = false;
/*
if (_parent_scene.m_DisableSlowPrims)
{
if (((float)fz == (float)(_parent_scene.gravityz * m_mass)) &&
(Math.Abs(vel.X) < 0.01 || Math.Abs(vel.Y) < 0.01 || Math.Abs(vel.Z) < 0.0001))
{
if (Math.Abs(vel.X) < 0.0001 || Math.Abs(vel.Y) < 0.0001 || Math.Abs(vel.Z) < 0.0001)
{
Vector3 angvelocity = new Vector3((float)angvel.X, (float)angvel.Y, (float)angvel.Z);
if (angvelocity.ApproxEquals(Vector3.Zero, 0.005f) &&
vel.X != 0 && vel.Y != 0 && vel.Z != 0)
{
if (d.BodyIsEnabled(Body))
{
d.BodySetLinearVel(Body, 0, 0, 0);
d.BodySetForce(Body, 0, 0, 0);
d.BodyDisable(Body);
disabled = true;
}
}
else
{
if (!d.BodyIsEnabled(Body))
d.BodyEnable(Body);
}
}
else
{
if (!d.BodyIsEnabled(Body))
{
d.BodyEnable(Body);
fz = 100 * m_mass;
}
}
}
}
*/
if (!disabled)
{
if (!d.BodyIsEnabled(Body))
{
enableBodySoft();
}
d.BodyAddForce(Body, fx, fy, fz);
d.BodyAddTorque(Body, newtorque.X, newtorque.Y, newtorque.Z);
}
}
}
}
else
{ // is not physical, or is not a body or is selected
// _zeroPosition = d.BodyGetPosition(Body);
return;
//Console.WriteLine("Nothing " + m_primName);
}
}
public virtual bool ErrorIsZero(Vector3 err)
{
return (err == Vector3.Zero || err.ApproxEquals(Vector3.Zero, ErrorZeroThreshold));
}
internal void ProcessGeomCreationAsTriMesh(Vector3 positionOffset, Quaternion orientation)
{
// Don't need to re-enable body.. it's done in SetMesh
float meshlod = _parent_scene.meshSculptLOD;
if (IsPhysical)
meshlod = _parent_scene.MeshSculptphysicalLOD;
IMesh mesh = _parent_scene.mesher.CreateMesh(SOPName, _pbs, _size, meshlod, IsPhysical);
if (!positionOffset.ApproxEquals(Vector3.Zero, 0.001f) || orientation != Quaternion.Identity)
{
float[] xyz = new float[3];
xyz[0] = positionOffset.X;
xyz[1] = positionOffset.Y;
xyz[2] = positionOffset.Z;
Matrix4 m4 = Matrix4.CreateFromQuaternion(orientation);
float[,] matrix = new float[3, 3];
matrix[0, 0] = m4.M11;
matrix[0, 1] = m4.M12;
matrix[0, 2] = m4.M13;
matrix[1, 0] = m4.M21;
matrix[1, 1] = m4.M22;
matrix[1, 2] = m4.M23;
matrix[2, 0] = m4.M31;
matrix[2, 1] = m4.M32;
matrix[2, 2] = m4.M33;
mesh.TransformLinear(matrix, xyz);
}
_mesh = mesh;
}
// =======================================================================
// =======================================================================
// Apply the effect of the angular motor.
// The 'contribution' is how much angular correction velocity each function wants.
// All the contributions are added together and the resulting velocity is
// set directly on the vehicle.
private void MoveAngular(float pTimestep)
{
// The user wants this many radians per second angular change?
Vector3 angularMotorContribution = m_angularMotor.Step(pTimestep);
// ==================================================================
// From http://wiki.secondlife.com/wiki/LlSetVehicleFlags :
// This flag prevents linear deflection parallel to world z-axis. This is useful
// for preventing ground vehicles with large linear deflection, like bumper cars,
// from climbing their linear deflection into the sky.
// That is, NO_DEFLECTION_UP says angular motion should not add any pitch or roll movement
if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
{
angularMotorContribution.X = 0f;
angularMotorContribution.Y = 0f;
VDetailLog("{0}, MoveAngular,noDeflectionUp,angularMotorContrib={1}", Prim.LocalID, angularMotorContribution);
}
Vector3 verticalAttractionContribution = ComputeAngularVerticalAttraction();
Vector3 deflectionContribution = ComputeAngularDeflection();
Vector3 bankingContribution = ComputeAngularBanking();
// ==================================================================
m_lastVertAttractor = verticalAttractionContribution;
m_lastAngularVelocity = angularMotorContribution
+ verticalAttractionContribution
+ deflectionContribution
+ bankingContribution;
// ==================================================================
// Apply the correction velocity.
// TODO: Should this be applied as an angular force (torque)?
if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
{
VehicleRotationalVelocity = m_lastAngularVelocity;
VDetailLog("{0}, MoveAngular,done,nonZero,angMotorContrib={1},vertAttrContrib={2},bankContrib={3},deflectContrib={4},totalContrib={5}",
Prim.LocalID,
angularMotorContribution, verticalAttractionContribution,
bankingContribution, deflectionContribution,
m_lastAngularVelocity
);
}
else
{
// The vehicle is not adding anything angular wise.
VehicleRotationalVelocity = Vector3.Zero;
VDetailLog("{0}, MoveAngular,done,zero", Prim.LocalID);
}
// ==================================================================
//Offset section
if (m_linearMotorOffset != Vector3.Zero)
{
//Offset of linear velocity doesn't change the linear velocity,
// but causes a torque to be applied, for example...
//
// IIIII >>> IIIII
// IIIII >>> IIIII
// IIIII >>> IIIII
// ^
// | Applying a force at the arrow will cause the object to move forward, but also rotate
//
//
// The torque created is the linear velocity crossed with the offset
// TODO: this computation should be in the linear section
// because that is where we know the impulse being applied.
Vector3 torqueFromOffset = Vector3.Zero;
// torqueFromOffset = Vector3.Cross(m_linearMotorOffset, appliedImpulse);
if (float.IsNaN(torqueFromOffset.X))
torqueFromOffset.X = 0;
if (float.IsNaN(torqueFromOffset.Y))
torqueFromOffset.Y = 0;
if (float.IsNaN(torqueFromOffset.Z))
torqueFromOffset.Z = 0;
VehicleAddAngularForce(torqueFromOffset * m_vehicleMass);
VDetailLog("{0}, BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", Prim.LocalID, torqueFromOffset);
}
}
} // end MoveLinear()
private void MoveAngular(float pTimestep)
{
/*
private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
private int m_angularMotorApply = 0; // application frame counter
private float m_angularMotorVelocity = 0; // current angular motor velocity (ramps up and down)
private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate
private float m_angularMotorDecayTimescale = 0; // motor 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
*/
// Get what the body is doing, this includes 'external' influences
d.Vector3 angularVelocity = d.BodyGetAngularVel(Body);
// Vector3 angularVelocity = Vector3.Zero;
if (m_angularMotorApply > 0)
{
// ramp up to new value
// current velocity += error / (time to get there / step interval)
// requested speed - last motor speed
m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep);
m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep);
m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep);
m_angularMotorApply--; // This is done so that if script request rate is less than phys frame rate the expected
// velocity may still be acheived.
}
else
{
// no motor recently applied, keep the body velocity
/* m_angularMotorVelocity.X = angularVelocity.X;
m_angularMotorVelocity.Y = angularVelocity.Y;
m_angularMotorVelocity.Z = angularVelocity.Z; */
// and decay the velocity
m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / pTimestep);
} // end motor section
// Vertical attractor section
Vector3 vertattr = Vector3.Zero;
if (m_verticalAttractionTimescale < 300)
{
float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep);
// get present body rotation
d.Quaternion rot = d.BodyGetQuaternion(Body);
Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
// make a vector pointing up
Vector3 verterr = Vector3.Zero;
verterr.Z = 1.0f;
// rotate it to Body Angle
verterr = verterr * rotq;
// verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1.
// As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go
// negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
if (verterr.Z < 0.0f)
{
verterr.X = 2.0f - verterr.X;
verterr.Y = 2.0f - verterr.Y;
}
// Error is 0 (no error) to +/- 2 (max error)
// scale it by VAservo
verterr = verterr * VAservo;
//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
// Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
vertattr.X = verterr.Y;
vertattr.Y = - verterr.X;
vertattr.Z = 0f;
// scaling appears better usingsquare-law
float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
vertattr.X += bounce * angularVelocity.X;
vertattr.Y += bounce * angularVelocity.Y;
} // else vertical attractor is off
// m_lastVertAttractor = vertattr;
// Bank section tba
// Deflection section tba
// Sum velocities
m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // + bank + deflection
if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
{
m_lastAngularVelocity.X = 0;
m_lastAngularVelocity.Y = 0;
}
if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
{
if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
}
else
{
m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
}
// apply friction
Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep);
m_lastAngularVelocity -= m_lastAngularVelocity * decayamount;
// Apply to the body
d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
} //end MoveAngular
// The user sets all the parameters and calls this which outputs values until error is zero.
public override void GenerateTestOutput(float timeStep)
{
// maximum number of outputs to generate.
int maxOutput = 50;
MDetailLog("{0},BSVMotor.Test,{1},===================================== BEGIN Test Output", BSScene.DetailLogZero, UseName);
MDetailLog("{0},BSVMotor.Test,{1},timeScale={2},targDlyTS={3},frictTS={4},eff={5},curr={6},tgt={7}",
BSScene.DetailLogZero, UseName,
TimeScale, TargetValueDecayTimeScale, FrictionTimescale, Efficiency,
CurrentValue, TargetValue);
LastError = BSMotor.InfiniteVector;
while (maxOutput-- > 0 && !LastError.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
{
Vector3 lastStep = Step(timeStep);
MDetailLog("{0},BSVMotor.Test,{1},cur={2},tgt={3},lastError={4},lastStep={5}",
BSScene.DetailLogZero, UseName, CurrentValue, TargetValue, LastError, lastStep);
}
MDetailLog("{0},BSVMotor.Test,{1},===================================== END Test Output", BSScene.DetailLogZero, UseName);
}
private void changeAngularLock(Vector3 newlock)
{
// do we have a Physical object?
if (Body != IntPtr.Zero)
{
//Check that we have a Parent
//If we have a parent then we're not authorative here
if (_parent == null)
{
if (!newlock.ApproxEquals(Vector3.One, 0f))
{
createAMotor(newlock);
}
else
{
if (Amotor != IntPtr.Zero)
{
d.JointDestroy(Amotor);
Amotor = IntPtr.Zero;
}
}
}
}
// Store this for later in case we get turned into a separate body
m_angularlock = newlock;
}
/// <summary>
/// Called from Simulate
/// This is the avatar's movement control + PID Controller
/// </summary>
/// <param name="timeStep"></param>
public void Move(float timeStep, List<AuroraODECharacter> defects)
{
// 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 (Body == IntPtr.Zero)
return;
// replace amotor
d.Quaternion dtmp;
dtmp.W =1;
dtmp.X=0;
dtmp.Y=0;
dtmp.Z=0;
d.BodySetQuaternion(Body, ref dtmp);
d.BodySetAngularVel(Body, 0, 0, 0);
if (m_pidControllerActive == false)
{
_zeroPosition = d.BodyGetPosition(Body);
}
//PidStatus = true;
// rex, added height check
d.Vector3 tempPos = d.BodyGetPosition(Body);
if (_parent_scene.m_useFlightCeilingHeight && tempPos.Z > _parent_scene.m_flightCeilingHeight)
{
tempPos.Z = _parent_scene.m_flightCeilingHeight;
d.BodySetPosition(Body, tempPos.X, tempPos.Y, tempPos.Z);
d.Vector3 tempVel = d.BodyGetLinearVel(Body);
if (tempVel.Z > 0.0f)
{
tempVel.Z = 0.0f;
d.BodySetLinearVel(Body, tempVel.X, tempVel.Y, tempVel.Z);
}
if (_target_velocity.Z > 0.0f)
_target_velocity.Z = 0.0f;
}
// endrex
Vector3 localPos = new Vector3((float)tempPos.X, (float)tempPos.Y, (float)tempPos.Z);
if (!localPos.IsFinite())
{
m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
defects.Add(this);
// _parent_scene.RemoveCharacter(this);
// destroy avatar capsule and related ODE data
if (Amotor != IntPtr.Zero)
{
// Kill the Amotor
d.JointDestroy(Amotor);
Amotor = IntPtr.Zero;
}
//kill the Geometry
_parent_scene.waitForSpaceUnlock(_parent_scene.space);
if (Body != IntPtr.Zero)
{
//kill the body
d.BodyDestroy(Body);
Body = IntPtr.Zero;
}
if (Shell != IntPtr.Zero)
{
d.GeomDestroy(Shell);
_parent_scene.geom_name_map.Remove(Shell);
Shell = IntPtr.Zero;
}
return;
}
Vector3 vec = Vector3.Zero;
d.Vector3 vel = d.BodyGetLinearVel(Body);
#region Check for underground
// _parent_scene.CheckTerrainColisionAABB(Shell);
// if (!flying || (flying && _target_velocity.X == 0 || _target_velocity.Y == 0))
// if (!m_iscollidingGround)
//Don't duplicate the ground check for flying from above, it will already have given us a good shove
{
// if (m_WaitGroundCheck >= 10 && vel.Z != 0)
{
float groundHeight = _parent_scene.GetTerrainHeightAtXY(tempPos.X, tempPos.Y);
if ((tempPos.Z - AvatarHalfsize) < groundHeight)
{
if (!flying)
{
if (_target_velocity.Z < 0)
_target_velocity.Z = 0;
vec.Z = -vel.Z * PID_D + ((groundHeight - (tempPos.Z - AvatarHalfsize)) * PID_P * 20.0f);
}
else
vec.Z = ((groundHeight - (tempPos.Z - AvatarHalfsize)) * PID_P);
}
if (tempPos.Z - AvatarHalfsize - groundHeight < 0.1)
{
m_iscolliding = true;
m_iscollidingGround = true;
flying = false; // gound the avatar
}
else
m_iscollidingGround = false;
// m_WaitGroundCheck = -1;
}
// m_WaitGroundCheck++;
}
/*
if (!m_alwaysRun)
movementdivisor = _parent_scene.avMovementDivisorWalk * (_parent_scene.TimeDilation < 0.3 ? 0.6f : _parent_scene.TimeDilation); //Dynamically adjust it for slower sims
else
movementdivisor = _parent_scene.avMovementDivisorRun * (_parent_scene.TimeDilation < 0.3 ? 0.6f : _parent_scene.TimeDilation); //Dynamically adjust it for slower sims
*/
// no dinamic messing here
float movementmult = 1f;
if (!m_alwaysRun)
movementmult /= _parent_scene.avMovementDivisorWalk;
else
movementmult /= _parent_scene.avMovementDivisorRun;
// if velocity is zero, use position control; otherwise, velocity control
if (_target_velocity == Vector3.Zero &&
Math.Abs(vel.X) < 0.05 && Math.Abs(vel.Y) < 0.05 && Math.Abs(vel.Z) < 0.05 && (this.m_iscollidingGround || this.m_iscollidingObj || this.flying))
//This is so that if we get moved by something else, it will update us in the client
{
// keep track of where we stopped. No more slippin' & slidin'
if (!_zeroFlag)
{
_zeroFlag = true;
_zeroPosition = tempPos;
}
if (m_pidControllerActive)
{
// 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
// if target vel is zero why was it here ?
//vec.X = -vel.X * PID_D + (_zeroPosition.X - tempPos.X) * PID_P * 2f;
//vec.Y = -vel.Y * PID_D + (_zeroPosition.Y - tempPos.Y) * PID_P * 2f;
}
}
else
{
m_pidControllerActive = true;
_zeroFlag = false;
if (m_iscolliding)
{
if(!flying)
{
if (_target_velocity.Z != 0.0f)
vec.Z = (_target_velocity.Z - vel.Z) * PID_D;// + (_zeroPosition.Z - tempPos.Z) * PID_P)) _zeropos maybe bad here
// We're standing or walking on something
vec.X = (_target_velocity.X * movementmult - vel.X) * PID_D*2;
vec.Y = (_target_velocity.Y * movementmult - vel.Y) * PID_D*2;
}
else
{
// We're flying and colliding with something
vec.X = (_target_velocity.X * movementmult - vel.X) * PID_D * 0.5f;
vec.Y = (_target_velocity.Y * movementmult - vel.Y) * PID_D * 0.5f;
}
}
else
{
if (flying)
{
// we're flyind
vec.X = (_target_velocity.X * movementmult - vel.X) * PID_D * 0.75f;
vec.Y = (_target_velocity.Y * movementmult - vel.Y) * PID_D * 0.75f;
}
else
{
// we're not colliding and we're not flying so that means we're falling!
// m_iscolliding includes collisions with the ground.
vec.X = (_target_velocity.X - vel.X) * PID_D * 0.85f;
vec.Y = (_target_velocity.Y - vel.Y) * PID_D * 0.85f;
}
}
if (flying)
{
#region Av gravity
if (_parent_scene.AllowAvGravity &&
tempPos.Z > _parent_scene.AvGravityHeight) //Should be stop avies from flying upwards
{
//Decay going up
if (_target_velocity.Z > 0)
{
//How much should we force them down?
float Multiplier = (_parent_scene.AllowAvsToEscapeGravity ? .03f : .1f);
//How much should we force them down?
float fudgeHeight = (_parent_scene.AllowAvsToEscapeGravity ? 80 : 30);
//We add the 30 so that gravity is resonably strong once they pass the min height
Multiplier *= tempPos.Z + fudgeHeight - _parent_scene.AvGravityHeight;
//Limit these so that things don't go wrong
if (Multiplier < 1)
Multiplier = 1;
float maxpower = (_parent_scene.AllowAvsToEscapeGravity ? 1.5f : 3f);
if (Multiplier > maxpower)
Multiplier = maxpower;
_target_velocity.Z /= Multiplier;
vel.Z /= Multiplier;
}
}
#endregion
vec.Z = (_target_velocity.Z - vel.Z) * PID_D * 0.5f;
if (_parent_scene.AllowAvGravity && tempPos.Z > _parent_scene.AvGravityHeight)
//Add extra gravity
vec.Z += ((10 * _parent_scene.gravityz) * Mass);
}
}
if (flying)
{
#region Auto Fly Height
//Added for auto fly height. Kitto Flora
//Changed to only check if the avatar is flying around,
// Revolution: If the avatar is going down, they are trying to land (probably), so don't push them up to make it harder
// Only if they are moving around sideways do we need to push them up
if (_target_velocity.X != 0 || _target_velocity.Y != 0)
{
Vector3 forwardVel = new Vector3(_target_velocity.X > 0 ? 2 : (_target_velocity.X < 0 ? -2 : 0),
_target_velocity.Y > 0 ? 2 : (_target_velocity.Y < 0 ? -2 : 0),
0);
float target_altitude = _parent_scene.GetTerrainHeightAtXY(tempPos.X, tempPos.Y) + MinimumGroundFlightOffset;
//We cheat a bit and do a bit lower than normal
if ((tempPos.Z - CAPSULE_LENGTH) < target_altitude ||
(tempPos.Z - CAPSULE_LENGTH) < _parent_scene.GetTerrainHeightAtXY(tempPos.X + forwardVel.X, tempPos.Y + forwardVel.Y)
+ MinimumGroundFlightOffset)
vec.Z += (target_altitude - tempPos.Z) * PID_P * 0.5f;
}
else
{
//Straight up and down, only apply when they are very close to the ground
float target_altitude = _parent_scene.GetTerrainHeightAtXY(tempPos.X, tempPos.Y);
if ((tempPos.Z - CAPSULE_LENGTH + (MinimumGroundFlightOffset / 1.5)) < target_altitude + MinimumGroundFlightOffset)
{
if ((tempPos.Z - CAPSULE_LENGTH) < target_altitude + 1)
{
vec.Z += ((target_altitude + 4) - (tempPos.Z - CAPSULE_LENGTH)) * PID_P;
}
else
vec.Z += ((target_altitude + MinimumGroundFlightOffset) - (tempPos.Z - CAPSULE_LENGTH)) * PID_P * 0.5f;
}
}
#endregion
}
#region Gravity
if (!flying && _parent_scene.AllowAvGravity)
{
if (!_parent_scene.UsePointGravity)
{
//Add normal gravity
vec.X += _parent_scene.gravityx * m_mass;
vec.Y += _parent_scene.gravityy * m_mass;
vec.Z += _parent_scene.gravityz * m_mass;
}
else
{
Vector3 cog = _parent_scene.PointOfGravity;
if (cog.X != 0)
vec.X += (cog.X - tempPos.X) * m_mass;
if (cog.Y != 0)
vec.Y += (cog.Y - tempPos.Y) * m_mass;
if (cog.Z != 0)
vec.Z += (cog.Z - tempPos.Z) * m_mass;
}
}
#endregion
#region Under water physics
if (_parent_scene.AllowUnderwaterPhysics)
{
//Position plus height to av's shoulder (aprox) is just above water
if ((tempPos.Z + (CAPSULE_LENGTH / 3) - .25f) < _parent_scene.GetWaterLevel((float)tempPos.X, (float)tempPos.Y))
{
if (StartingUnderWater)
ShouldBeWalking = Flying == false;
StartingUnderWater = false;
WasUnderWater = true;
Flying = true;
lastUnderwaterPush = 0;
if (ShouldBeWalking)
{
lastUnderwaterPush += (float)(_parent_scene.GetWaterLevel((float)tempPos.X, (float)tempPos.Y) - tempPos.Z) * 33 + 3;
vec.Z += lastUnderwaterPush;
}
else
{
lastUnderwaterPush += 3500;
lastUnderwaterPush += (float)(_parent_scene.GetWaterLevel((float)tempPos.X, (float)tempPos.Y) - tempPos.Z) * 8;
vec.Z += lastUnderwaterPush;
}
}
else
{
StartingUnderWater = true;
if (WasUnderWater)
{
WasUnderWater = false;
Flying = true;
}
}
}
#endregion
#endregion
if (vec.IsFinite())
{
if (vec.X < 100000000 && vec.Y < 10000000 && vec.Z < 10000000) //Checks for crazy, going to NaN us values
{
d.Vector3 veloc = d.BodyGetLinearVel(Body);
//Stop us from fidgiting if we have a small velocity
/*
if (_zeroFlag && ((Math.Abs(vec.X) < 0.09 && Math.Abs(vec.Y) < 0.09 && Math.Abs(vec.Z) < 0.03) && !flying && vec.Z != 0))
{
//m_log.Warn("Nulling Velo: " + vec.ToString());
vec = new Vector3(0, 0, 0);
d.BodySetLinearVel(Body, 0, 0, 0);
}
//Reduce insanely small values to 0 if the velocity isn't going up
if (Math.Abs(vec.Z) < 0.01 && veloc.Z < 0.6 && _zeroFlag)
{
if (veloc.Z != 0)
{
if (-veloc.Z > 0)
vec.Z = 0;
else
vec.Z = -veloc.Z * 5;
d.BodySetLinearVel(Body, veloc.X, veloc.Y, vec.Z);
}
}
*/
// round small values to zero. those possible are just errors
if (Math.Abs(vec.X) < 0.001)
vec.X = 0;
if (Math.Abs(vec.Y) < 0.001)
vec.Y = 0;
if (Math.Abs(vec.Z) < 0.001)
vec.Z = 0;
doForce(vec);
//When falling, we keep going faster and faster, and eventually, the client blue screens (blue is all you see).
// The speed that does this is slightly higher than -30, so we cap it here so we never do that during falling.
if (vel.Z < -30)
{
vel.Z = -30;
d.BodySetLinearVel(Body, vel.X, vel.Y, vel.Z);
}
//Decay out the target velocity
_target_velocity *= _parent_scene.m_avDecayTime;
if (!_zeroFlag && _target_velocity.ApproxEquals(Vector3.Zero, _parent_scene.m_avStopDecaying))
_target_velocity = Vector3.Zero;
//Check if the capsule is tilted before changing it
// if (!_zeroFlag && !_parent_scene.IsAvCapsuleTilted)
// AlignAvatarTiltWithCurrentDirectionOfMovement(vec);
}
else
{
//This is a safe guard from going NaN, but it isn't very smooth... which is ok
d.BodySetForce(Body, 0, 0, 0);
d.BodySetLinearVel(Body, 0, 0, 0);
}
}
else
{
m_log.Warn("[PHYSICS]: Got a NaN force vector in Move()");
m_log.Warn("[PHYSICS]: Avatar Position is non-finite!");
defects.Add(this);
// _parent_scene.RemoveCharacter(this);
// destroy avatar capsule and related ODE data
if (Amotor != IntPtr.Zero)
{
// Kill the Amotor
d.JointDestroy(Amotor);
Amotor = IntPtr.Zero;
}
//kill the Geometry
_parent_scene.waitForSpaceUnlock(_parent_scene.space);
if (Body != IntPtr.Zero)
{
//kill the body
d.BodyDestroy(Body);
Body = IntPtr.Zero;
}
if (Shell != IntPtr.Zero)
{
d.GeomDestroy(Shell);
_parent_scene.geom_name_map.Remove(Shell);
Shell = IntPtr.Zero;
}
}
}
public void Move(float timestep)
{
float fx = 0;
float fy = 0;
float fz = 0;
if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims.
{
if (m_vehicle.Type != Vehicle.TYPE_NONE)
{
// 'VEHICLES' are dealt with in ODEDynamics.cs
m_vehicle.Step(timestep, _parent_scene);
}
else
{
//Console.WriteLine("Move " + Name);
if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
// NON-'VEHICLES' are dealt with here
// if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f))
// {
// d.Vector3 avel2 = d.BodyGetAngularVel(Body);
// /*
// if (m_angularlock.X == 1)
// avel2.X = 0;
// if (m_angularlock.Y == 1)
// avel2.Y = 0;
// if (m_angularlock.Z == 1)
// avel2.Z = 0;
// d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
// */
// }
//float PID_P = 900.0f;
float m_mass = CalculateMass();
// fz = 0f;
//m_log.Info(m_collisionFlags.ToString());
//KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
// would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ??
// m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
// gravityz multiplier = 1 - m_buoyancy
fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass;
if (m_usePID)
{
//Console.WriteLine("PID " + Name);
// KF - this is for object move? eg. llSetPos() ?
//if (!d.BodyIsEnabled(Body))
//d.BodySetForce(Body, 0f, 0f, 0f);
// If we're using the PID controller, then we have no gravity
//fz = (-1 * _parent_scene.gravityz) * m_mass; //KF: ?? Prims have no global gravity,so simply...
fz = 0f;
// 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) && (m_PIDTau != 0))
{
//PID_G = PID_G / m_PIDTau;
m_PIDTau = 1;
}
if ((PID_G - m_PIDTau) <= 0)
{
PID_G = m_PIDTau + 1;
}
//PidStatus = true;
// PhysicsVector vec = new PhysicsVector();
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 = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
}
} // end if (m_usePID)
// Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
if (m_useHoverPID && !m_usePID)
{
//Console.WriteLine("Hover " + Name);
// If we're using the PID controller, then we have no gravity
fz = (-1 * _parent_scene.gravityz) * m_mass;
// 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);
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)
{
m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
}
else
{
m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
}
break;
} // end switch (m_PIDHoverType)
_target_velocity =
new Vector3(0.0f, 0.0f,
(m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * 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
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 + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
}
}
fx *= m_mass;
fy *= m_mass;
//fz *= m_mass;
fx += m_force.X;
fy += m_force.Y;
fz += m_force.Z;
//m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
if (fx != 0 || fy != 0 || fz != 0)
{
//m_taintdisable = true;
//base.RaiseOutOfBounds(Position);
//d.BodySetLinearVel(Body, fx, fy, 0f);
if (!d.BodyIsEnabled(Body))
{
// A physical body at rest on a surface will auto-disable after a while,
// this appears to re-enable it incase the surface it is upon vanishes,
// and the body should fall again.
d.BodySetLinearVel(Body, 0f, 0f, 0f);
d.BodySetForce(Body, 0, 0, 0);
enableBodySoft();
}
// 35x10 = 350n times the mass per second applied maximum.
float nmax = 35f * m_mass;
float nmin = -35f * m_mass;
if (fx > nmax)
fx = nmax;
if (fx < nmin)
fx = nmin;
if (fy > nmax)
fy = nmax;
if (fy < nmin)
fy = nmin;
d.BodyAddForce(Body, fx, fy, fz);
//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
}
}
}
else
{ // is not physical, or is not a body or is selected
// _zeroPosition = d.BodyGetPosition(Body);
return;
//Console.WriteLine("Nothing " + Name);
}
}
} // end MoveLinear()
// Apply the effect of the angular motor.
private void MoveAngular(float pTimestep)
{
// m_angularMotorDirection // angular velocity requested by LSL motor
// m_angularMotorApply // application frame counter
// m_angularMotorVelocity // current angular motor velocity (ramps up and down)
// m_angularMotorTimescale // motor angular velocity ramp up rate
// m_angularMotorDecayTimescale // motor angular velocity decay rate
// m_angularFrictionTimescale // body angular velocity decay rate
// m_lastAngularVelocity // what was last applied to body
// Get what the body is doing, this includes 'external' influences
Vector3 angularVelocity = Prim.ForceRotationalVelocity;
if (m_angularMotorApply > 0)
{
// Rather than snapping the angular motor velocity from the old value to
// a newly set velocity, this routine steps the value from the previous
// value (m_angularMotorVelocity) to the requested value (m_angularMotorDirection).
// There are m_angularMotorApply steps.
Vector3 origVel = m_angularMotorVelocity;
Vector3 origDir = m_angularMotorDirection;
// ramp up to new value
// new velocity += error / ( time to get there / step interval)
// requested speed - last motor speed
m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep);
m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep);
m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep);
VDetailLog("{0},MoveAngular,angularMotorApply,apply={1},angTScale={2},timeStep={3},origvel={4},origDir={5},vel={6}",
Prim.LocalID, m_angularMotorApply, m_angularMotorTimescale, pTimestep, origVel, origDir, m_angularMotorVelocity);
m_angularMotorApply--;
}
else
{
// No motor recently applied, keep the body velocity
// and decay the velocity
m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / pTimestep);
if (m_angularMotorVelocity.LengthSquared() < 0.00001)
m_angularMotorVelocity = Vector3.Zero;
} // end motor section
// Vertical attractor section
Vector3 vertattr = Vector3.Zero;
Vector3 deflection = Vector3.Zero;
Vector3 banking = Vector3.Zero;
if (m_verticalAttractionTimescale < 300 && m_lastAngularVelocity != Vector3.Zero)
{
float VAservo = 0.2f / (m_verticalAttractionTimescale / pTimestep);
VAservo *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
// get present body rotation
Quaternion rotq = Prim.ForceOrientation;
// vector pointing up
Vector3 verterr = Vector3.Zero;
verterr.Z = 1.0f;
// rotate it to Body Angle
verterr = verterr * rotq;
// verterr.X and .Y are the World error amounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1.
// As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go
// negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
// Error is 0 (no error) to +/- 2 (max error)
if (verterr.Z < 0.0f)
{
verterr.X = 2.0f - verterr.X;
verterr.Y = 2.0f - verterr.Y;
}
// scale it by VAservo
verterr = verterr * VAservo;
// 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.
vertattr.X = verterr.Y;
vertattr.Y = - verterr.X;
vertattr.Z = 0f;
// scaling appears better usingsquare-law
float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
vertattr.X += bounce * angularVelocity.X;
vertattr.Y += bounce * angularVelocity.Y;
VDetailLog("{0},MoveAngular,verticalAttraction,verterr={1},bounce={2},vertattr={3}",
Prim.LocalID, verterr, bounce, vertattr);
} // else vertical attractor is off
m_lastVertAttractor = vertattr;
// Bank section tba
// Deflection section tba
// Sum velocities
m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // + bank + deflection
if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
{
m_lastAngularVelocity.X = 0;
m_lastAngularVelocity.Y = 0;
VDetailLog("{0},MoveAngular,noDeflectionUp,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
}
if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
{
m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
VDetailLog("{0},MoveAngular,zeroSmallValues,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
}
// apply friction
Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep);
m_lastAngularVelocity -= m_lastAngularVelocity * decayamount;
// Apply to the body
Prim.ForceRotationalVelocity = m_lastAngularVelocity;
VDetailLog("{0},MoveAngular,done,decay={1},lastAngular={2}", Prim.LocalID, decayamount, m_lastAngularVelocity);
} //end MoveAngular
public void Move(float timestep)
{
if (!childPrim && m_isphysical && Body != IntPtr.Zero &&
!m_disabled && !m_isSelected && d.BodyIsEnabled(Body) && !m_building) // KF: Only move root prims.
{
// if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009
float fx = 0;
float fy = 0;
float fz = 0;
if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
{
// 'VEHICLES' are dealt with in ODEDynamics.cs
m_vehicle.Step(timestep, _parent_scene,Body);
}
else
{
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);
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)
{
m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
}
else
{
m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
}
break;
} // end switch (m_PIDHoverType)
_target_velocity =
new Vector3(0.0f, 0.0f,
(m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * 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
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 = ((_target_velocity.Z - 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);
}
}
}
else
{ // is not physical, or is not a body or is selected
// _zeroPosition = d.BodyGetPosition(Body);
return;
//Console.WriteLine("Nothing " + Name);
}
}
public void TestLinkDelink2SceneObjects()
{
TestHelpers.InMethod();
bool debugtest = false;
Scene scene = SceneHelpers.SetupScene();
SceneObjectPart part1 = SceneHelpers.AddSceneObject(scene);
SceneObjectGroup grp1 = part1.ParentGroup;
SceneObjectPart part2 = SceneHelpers.AddSceneObject(scene);
SceneObjectGroup grp2 = part2.ParentGroup;
grp1.AbsolutePosition = new Vector3(10, 10, 10);
grp2.AbsolutePosition = Vector3.Zero;
// <90,0,0>
// grp1.UpdateGroupRotationR(Quaternion.CreateFromEulers(90 * Utils.DEG_TO_RAD, 0, 0));
// <180,0,0>
grp2.UpdateGroupRotationR(Quaternion.CreateFromEulers(180 * Utils.DEG_TO_RAD, 0, 0));
// Required for linking
grp1.RootPart.ClearUpdateSchedule();
grp2.RootPart.ClearUpdateSchedule();
// Link grp2 to grp1. part2 becomes child prim to grp1. grp2 is eliminated.
grp1.LinkToGroup(grp2);
// FIXME: Can't do this test yet since group 2 still has its root part! We can't yet null this since
// it might cause SOG.ProcessBackup() to fail due to the race condition. This really needs to be fixed.
Assert.That(grp2.IsDeleted, "SOG 2 was not registered as deleted after link.");
Assert.That(grp2.Parts.Length, Is.EqualTo(0), "Group 2 still contained children after delink.");
Assert.That(grp1.Parts.Length == 2);
if (debugtest)
{
m_log.Debug("parts: " + grp1.Parts.Length);
m_log.Debug("Group1: Pos:"+grp1.AbsolutePosition+", Rot:"+grp1.GroupRotation);
m_log.Debug("Group1: Prim1: OffsetPosition:"+ part1.OffsetPosition+", OffsetRotation:"+part1.RotationOffset);
m_log.Debug("Group1: Prim2: OffsetPosition:"+part2.OffsetPosition+", OffsetRotation:"+part2.RotationOffset);
}
// root part should have no offset position or rotation
Assert.That(part1.OffsetPosition == Vector3.Zero && part1.RotationOffset == Quaternion.Identity,
"root part should have no offset position or rotation");
// offset position should be root part position - part2.absolute position.
Assert.That(part2.OffsetPosition == new Vector3(-10, -10, -10),
"offset position should be root part position - part2.absolute position.");
float roll = 0;
float pitch = 0;
float yaw = 0;
// There's a euler anomoly at 180, 0, 0 so expect 180 to turn into -180.
part1.RotationOffset.GetEulerAngles(out roll, out pitch, out yaw);
Vector3 rotEuler1 = new Vector3(roll * Utils.RAD_TO_DEG, pitch * Utils.RAD_TO_DEG, yaw * Utils.RAD_TO_DEG);
if (debugtest)
m_log.Debug(rotEuler1);
part2.RotationOffset.GetEulerAngles(out roll, out pitch, out yaw);
Vector3 rotEuler2 = new Vector3(roll * Utils.RAD_TO_DEG, pitch * Utils.RAD_TO_DEG, yaw * Utils.RAD_TO_DEG);
if (debugtest)
m_log.Debug(rotEuler2);
Assert.That(rotEuler2.ApproxEquals(new Vector3(-180, 0, 0), 0.001f) || rotEuler2.ApproxEquals(new Vector3(180, 0, 0), 0.001f),
"Not exactly sure what this is asserting...");
// Delink part 2
SceneObjectGroup grp3 = grp1.DelinkFromGroup(part2.LocalId);
if (debugtest)
m_log.Debug("Group2: Prim2: OffsetPosition:" + part2.AbsolutePosition + ", OffsetRotation:" + part2.RotationOffset);
Assert.That(grp1.Parts.Length, Is.EqualTo(1), "Group 1 still contained part2 after delink.");
Assert.That(part2.AbsolutePosition == Vector3.Zero, "The absolute position should be zero");
Assert.That(grp3.HasGroupChangedDueToDelink, Is.True);
}
public virtual Vector3 Step(float timeStep, Vector3 error)
{
if (!Enabled) return Vector3.Zero;
LastError = error;
Vector3 returnCorrection = Vector3.Zero;
if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
{
// correction = error / secondsItShouldTakeToCorrect
Vector3 correctionAmount;
if (TimeScale == 0f || TimeScale == BSMotor.Infinite)
correctionAmount = error * timeStep;
else
correctionAmount = error / TimeScale * timeStep;
returnCorrection = correctionAmount;
MDetailLog("{0}, BSVMotor.Step,nonZero,{1},timeStep={2},timeScale={3},err={4},corr={5}",
BSScene.DetailLogZero, UseName, timeStep, TimeScale, error, correctionAmount);
}
return returnCorrection;
}
