aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs676
1 files changed, 324 insertions, 352 deletions
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
index 49766f8..7c0bbef 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
@@ -98,10 +98,12 @@ namespace OpenSim.Region.Physics.OdePlugin
98 private Vector3 m_forceacc; 98 private Vector3 m_forceacc;
99 private Vector3 m_angularForceacc; 99 private Vector3 m_angularForceacc;
100 100
101 private float m_invTimeStep = 50.0f;
102 private float m_timeStep = .02f;
103
104
101 private Vector3 m_PIDTarget; 105 private Vector3 m_PIDTarget;
102 private float m_PIDTau; 106 private float m_PIDTau;
103 private float PID_D = 35f;
104 private float PID_G = 25f;
105 private bool m_usePID; 107 private bool m_usePID;
106 108
107 // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), 109 // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
@@ -632,7 +634,6 @@ namespace OpenSim.Region.Physics.OdePlugin
632 Vector3 pv = Vector3.Zero; 634 Vector3 pv = Vector3.Zero;
633 if (_zeroFlag) 635 if (_zeroFlag)
634 return pv; 636 return pv;
635 m_lastUpdateSent = false;
636 637
637 if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) 638 if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f))
638 return pv; 639 return pv;
@@ -686,12 +687,50 @@ namespace OpenSim.Region.Physics.OdePlugin
686 } 687 }
687 688
688 public override bool PIDActive { set { m_usePID = value; } } 689 public override bool PIDActive { set { m_usePID = value; } }
689 public override float PIDTau { set { m_PIDTau = value; } } 690 public override float PIDTau
691 {
692 set
693 {
694 if (value <= 0)
695 m_PIDTau = 0;
696 else
697 {
698 float mint = (0.05f > _parent_scene.ODE_STEPSIZE ? 0.05f : _parent_scene.ODE_STEPSIZE);
699 if (value < mint)
700 m_PIDTau = mint;
701 else
702 m_PIDTau = value;
703 }
704 }
705 }
690 706
691 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } 707 public override float PIDHoverHeight
708 {
709 set
710 {
711 m_PIDHoverHeight = value;
712 if (value == 0)
713 m_useHoverPID = false;
714 }
715 }
692 public override bool PIDHoverActive { set { m_useHoverPID = value; } } 716 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
693 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } 717 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
694 public override float PIDHoverTau { set { m_PIDHoverTau = value; } } 718 public override float PIDHoverTau
719 {
720 set
721 {
722 if (value <= 0)
723 m_PIDHoverTau = 0;
724 else
725 {
726 float mint = (0.05f > _parent_scene.ODE_STEPSIZE ? 0.05f : _parent_scene.ODE_STEPSIZE);
727 if (value < mint)
728 m_PIDHoverTau = mint;
729 else
730 m_PIDHoverTau = value;
731 }
732 }
733 }
695 734
696 public override Quaternion APIDTarget { set { return; } } 735 public override Quaternion APIDTarget { set { return; } }
697 736
@@ -912,8 +951,9 @@ namespace OpenSim.Region.Physics.OdePlugin
912 _position = pos; 951 _position = pos;
913 givefakepos = 0; 952 givefakepos = 0;
914 953
915 PID_D = parent_scene.bodyPIDD; 954 m_timeStep = parent_scene.ODE_STEPSIZE;
916 PID_G = parent_scene.bodyPIDG; 955 m_invTimeStep = 1f / m_timeStep;
956
917 m_density = parent_scene.geomDefaultDensity; 957 m_density = parent_scene.geomDefaultDensity;
918 // m_tensor = parent_scene.bodyMotorJointMaxforceTensor; 958 // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
919 body_autodisable_frames = parent_scene.bodyFramesAutoDisable; 959 body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
@@ -2696,12 +2736,6 @@ namespace OpenSim.Region.Physics.OdePlugin
2696 2736
2697 private void DoSelectedStatus(bool newval) 2737 private void DoSelectedStatus(bool newval)
2698 { 2738 {
2699 if (m_isSelected == newval)
2700 {
2701 resetCollisionAccounting();
2702 return;
2703 }
2704
2705 m_isSelected = newval; 2739 m_isSelected = newval;
2706 Stop(); 2740 Stop();
2707 2741
@@ -2970,7 +3004,6 @@ namespace OpenSim.Region.Physics.OdePlugin
2970 givefakeori--; 3004 givefakeori--;
2971 if (givefakeori < 0) 3005 if (givefakeori < 0)
2972 givefakeori = 0; 3006 givefakeori = 0;
2973
2974 resetCollisionAccounting(); 3007 resetCollisionAccounting();
2975 } 3008 }
2976 3009
@@ -3125,9 +3158,10 @@ namespace OpenSim.Region.Physics.OdePlugin
3125 d.BodyEnable(Body); 3158 d.BodyEnable(Body);
3126 } 3159 }
3127 3160
3128 private void changeAddForce(Vector3 force) 3161
3162 private void changeAddImpulse(Vector3 impulse)
3129 { 3163 {
3130 m_forceacc += force; 3164 m_forceacc += impulse * m_invTimeStep;
3131 if (!m_isSelected) 3165 if (!m_isSelected)
3132 { 3166 {
3133 lock (this) 3167 lock (this)
@@ -3146,9 +3180,10 @@ namespace OpenSim.Region.Physics.OdePlugin
3146 } 3180 }
3147 } 3181 }
3148 3182
3149 private void changeAddAngularForce(Vector3 aforce) 3183 // actually angular impulse
3184 private void changeAddAngularImpulse(Vector3 aimpulse)
3150 { 3185 {
3151 m_angularForceacc += aforce; 3186 m_angularForceacc += aimpulse * m_invTimeStep;
3152 if (!m_isSelected) 3187 if (!m_isSelected)
3153 { 3188 {
3154 lock (this) 3189 lock (this)
@@ -3271,330 +3306,246 @@ namespace OpenSim.Region.Physics.OdePlugin
3271 if (!childPrim && m_isphysical && Body != IntPtr.Zero && 3306 if (!childPrim && m_isphysical && Body != IntPtr.Zero &&
3272 !m_disabled && !m_isSelected && !m_building && !m_outbounds) 3307 !m_disabled && !m_isSelected && !m_building && !m_outbounds)
3273 { 3308 {
3274 if (d.BodyIsEnabled(Body)) 3309 if (!d.BodyIsEnabled(Body))
3275 { 3310 {
3276 float timestep = _parent_scene.ODE_STEPSIZE; 3311 // let vehicles sleep
3277 3312 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
3278 // check outside region
3279 d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator
3280
3281 if (lpos.Z < -100 || lpos.Z > 100000f)
3282 {
3283 m_outbounds = true;
3284
3285 lpos.Z = Util.Clip(lpos.Z, -100f, 100000f);
3286 _acceleration.X = 0;
3287 _acceleration.Y = 0;
3288 _acceleration.Z = 0;
3289
3290 _velocity.X = 0;
3291 _velocity.Y = 0;
3292 _velocity.Z = 0;
3293 m_rotationalVelocity.X = 0;
3294 m_rotationalVelocity.Y = 0;
3295 m_rotationalVelocity.Z = 0;
3296
3297 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
3298 d.BodySetAngularVel(Body, 0, 0, 0); // stop it
3299 d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere
3300 m_lastposition = _position;
3301 m_lastorientation = _orientation;
3302
3303 base.RequestPhysicsterseUpdate();
3304
3305 m_throttleUpdates = false;
3306 throttleCounter = 0;
3307 _zeroFlag = true;
3308
3309 disableBodySoft(); // disable it and colisions
3310 base.RaiseOutOfBounds(_position);
3311 return; 3313 return;
3312 }
3313 3314
3314 if (lpos.X < 0f) 3315 if (++bodydisablecontrol < 20)
3315 {
3316 _position.X = Util.Clip(lpos.X, -2f, -0.1f);
3317 m_outbounds = true;
3318 }
3319 else if (lpos.X > _parent_scene.WorldExtents.X)
3320 {
3321 _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f);
3322 m_outbounds = true;
3323 }
3324 if (lpos.Y < 0f)
3325 {
3326 _position.Y = Util.Clip(lpos.Y, -2f, -0.1f);
3327 m_outbounds = true;
3328 }
3329 else if (lpos.Y > _parent_scene.WorldExtents.Y)
3330 {
3331 _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f);
3332 m_outbounds = true;
3333 }
3334
3335 if (m_outbounds)
3336 {
3337 m_lastposition = _position;
3338 m_lastorientation = _orientation;
3339
3340 d.Vector3 dtmp = d.BodyGetAngularVel(Body);
3341 m_rotationalVelocity.X = dtmp.X;
3342 m_rotationalVelocity.Y = dtmp.Y;
3343 m_rotationalVelocity.Z = dtmp.Z;
3344
3345 dtmp = d.BodyGetLinearVel(Body);
3346 _velocity.X = dtmp.X;
3347 _velocity.Y = dtmp.Y;
3348 _velocity.Z = dtmp.Z;
3349
3350 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
3351 d.BodySetAngularVel(Body, 0, 0, 0);
3352 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
3353 disableBodySoft(); // stop collisions
3354 base.RequestPhysicsterseUpdate();
3355 return; 3316 return;
3356 }
3357 3317
3358 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) 3318 bodydisablecontrol = 0;
3359 { 3319 d.BodyEnable(Body);
3360 // 'VEHICLES' are dealt with in ODEDynamics.cs 3320 }
3361 m_vehicle.Step();
3362 }
3363 else
3364 {
3365 float fx = 0;
3366 float fy = 0;
3367 float fz = 0;
3368
3369 float m_mass = _mass;
3370
3371 // fz = 0f;
3372 //m_log.Info(m_collisionFlags.ToString());
3373 if (m_usePID)
3374 {
3375 3321
3376 // If the PID Controller isn't active then we set our force 3322 // check outside region
3377 // calculating base velocity to the current position 3323 d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator
3378 3324
3379 if ((m_PIDTau < 1) && (m_PIDTau != 0)) 3325 if (lpos.Z < -100 || lpos.Z > 100000f)
3380 { 3326 {
3381 //PID_G = PID_G / m_PIDTau; 3327 m_outbounds = true;
3382 m_PIDTau = 1;
3383 }
3384 3328
3385 if ((PID_G - m_PIDTau) <= 0) 3329 lpos.Z = Util.Clip(lpos.Z, -100f, 100000f);
3386 { 3330 _acceleration.X = 0;
3387 PID_G = m_PIDTau + 1; 3331 _acceleration.Y = 0;
3388 } 3332 _acceleration.Z = 0;
3389 3333
3390 d.Vector3 vel = d.BodyGetLinearVel(Body); 3334 _velocity.X = 0;
3391 d.Vector3 pos = d.BodyGetPosition(Body); 3335 _velocity.Y = 0;
3392 _target_velocity = 3336 _velocity.Z = 0;
3393 new Vector3( 3337 m_rotationalVelocity.X = 0;
3394 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), 3338 m_rotationalVelocity.Y = 0;
3395 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), 3339 m_rotationalVelocity.Z = 0;
3396 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
3397 );
3398 3340
3399 // if velocity is zero, use position control; otherwise, velocity control 3341 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
3342 d.BodySetAngularVel(Body, 0, 0, 0); // stop it
3343 d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere
3344 m_lastposition = _position;
3345 m_lastorientation = _orientation;
3400 3346
3401 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) 3347 base.RequestPhysicsterseUpdate();
3402 {
3403 // keep track of where we stopped. No more slippin' & slidin'
3404
3405 // We only want to deactivate the PID Controller if we think we want to have our surrogate
3406 // react to the physics scene by moving it's position.
3407 // Avatar to Avatar collisions
3408 // Prim to avatar collisions
3409
3410 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
3411 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
3412 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
3413 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3414 d.BodySetLinearVel(Body, 0, 0, 0);
3415 // d.BodyAddForce(Body, 0, 0, fz);
3416 return;
3417 }
3418 else
3419 {
3420 _zeroFlag = false;
3421 3348
3422 // We're flying and colliding with something 3349 throttleCounter = 0;
3423 fx = ((_target_velocity.X) - vel.X) * (PID_D); 3350 _zeroFlag = true;
3424 fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
3425 3351
3426 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; 3352 disableBodySoft(); // disable it and colisions
3353 base.RaiseOutOfBounds(_position);
3354 return;
3355 }
3427 3356
3428 fz = ((_target_velocity.Z - vel.Z) * (PID_D)); 3357 if (lpos.X < 0f)
3429 } 3358 {
3430 } // end if (m_usePID) 3359 _position.X = Util.Clip(lpos.X, -2f, -0.1f);
3360 m_outbounds = true;
3361 }
3362 else if (lpos.X > _parent_scene.WorldExtents.X)
3363 {
3364 _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f);
3365 m_outbounds = true;
3366 }
3367 if (lpos.Y < 0f)
3368 {
3369 _position.Y = Util.Clip(lpos.Y, -2f, -0.1f);
3370 m_outbounds = true;
3371 }
3372 else if (lpos.Y > _parent_scene.WorldExtents.Y)
3373 {
3374 _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f);
3375 m_outbounds = true;
3376 }
3431 3377
3432 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller 3378 if (m_outbounds)
3433 else if (m_useHoverPID) 3379 {
3434 { 3380 m_lastposition = _position;
3435 //Console.WriteLine("Hover " + Name); 3381 m_lastorientation = _orientation;
3436 3382
3437 // If we're using the PID controller, then we have no gravity 3383 d.Vector3 dtmp = d.BodyGetAngularVel(Body);
3384 m_rotationalVelocity.X = dtmp.X;
3385 m_rotationalVelocity.Y = dtmp.Y;
3386 m_rotationalVelocity.Z = dtmp.Z;
3438 3387
3439 // no lock; for now it's only called from within Simulate() 3388 dtmp = d.BodyGetLinearVel(Body);
3389 _velocity.X = dtmp.X;
3390 _velocity.Y = dtmp.Y;
3391 _velocity.Z = dtmp.Z;
3440 3392
3441 // If the PID Controller isn't active then we set our force 3393 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
3442 // calculating base velocity to the current position 3394 d.BodySetAngularVel(Body, 0, 0, 0);
3395 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
3396 disableBodySoft(); // stop collisions
3397 base.RequestPhysicsterseUpdate();
3398 return;
3399 }
3443 3400
3444 if ((m_PIDTau < 1)) 3401 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
3445 { 3402 {
3446 PID_G = PID_G / m_PIDTau; 3403 // 'VEHICLES' are dealt with in ODEDynamics.cs
3447 } 3404 m_vehicle.Step();
3405 return;
3406 }
3448 3407
3449 if ((PID_G - m_PIDTau) <= 0) 3408 float fx = 0;
3450 { 3409 float fy = 0;
3451 PID_G = m_PIDTau + 1; 3410 float fz = 0;
3452 }
3453 3411
3454 // Where are we, and where are we headed? 3412 float m_mass = _mass;
3455 d.Vector3 pos = d.BodyGetPosition(Body);
3456 d.Vector3 vel = d.BodyGetLinearVel(Body);
3457 3413
3458 // Non-Vehicles have a limited set of Hover options. 3414 if (m_usePID && m_PIDTau > 0)
3459 // determine what our target height really is based on HoverType 3415 {
3460 switch (m_PIDHoverType) 3416 // for now position error
3461 { 3417 _target_velocity =
3462 case PIDHoverType.Ground: 3418 new Vector3(
3463 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); 3419 (m_PIDTarget.X - lpos.X),
3420 (m_PIDTarget.Y - lpos.Y),
3421 (m_PIDTarget.Z - lpos.Z)
3422 );
3464 3423
3465 break; 3424 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.02f))
3466 case PIDHoverType.GroundAndWater: 3425 {
3467 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); 3426 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3468 m_waterHeight = _parent_scene.GetWaterLevel(); 3427 d.BodySetLinearVel(Body, 0, 0, 0);
3469 if (m_groundHeight > m_waterHeight) 3428 return;
3470 { 3429 }
3471 if (m_PIDHoverHeight > 0 || m_isVolumeDetect) 3430 else
3472 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; 3431 {
3473 else 3432 _zeroFlag = false;
3474 m_targetHoverHeight = m_groundHeight;
3475 }
3476 else
3477 {
3478 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
3479 }
3480 break;
3481 3433
3482 } // end switch (m_PIDHoverType) 3434 float tmp = 1 / m_PIDTau;
3435 _target_velocity *= tmp;
3483 3436
3484 // don't go underground unless volumedetector 3437 // apply limits
3485 3438 tmp = _target_velocity.Length();
3486 if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect) 3439 if (tmp > 50.0f)
3487 { 3440 {
3488 fz = (m_targetHoverHeight - pos.Z) * (PID_G - m_PIDHoverTau) * timestep; 3441 tmp = 50 / tmp;
3442 _target_velocity *= tmp;
3443 }
3444 else if (tmp < 0.05f)
3445 {
3446 tmp = 0.05f / tmp;
3447 _target_velocity *= tmp;
3448 }
3489 3449
3490 // if velocity is zero, use position control; otherwise, velocity control 3450 d.Vector3 vel = d.BodyGetLinearVel(Body);
3451 fx = (_target_velocity.X - vel.X) * m_invTimeStep;
3452 fy = (_target_velocity.Y - vel.Y) * m_invTimeStep;
3453 fz = (_target_velocity.Z - vel.Z) * m_invTimeStep;
3454 }
3455 } // end if (m_usePID)
3491 3456
3492 if (Math.Abs(fz) < 0.1f) 3457 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
3493 { 3458 else if (m_useHoverPID && m_PIDHoverTau != 0 && m_PIDHoverHeight != 0)
3494 // keep track of where we stopped. No more slippin' & slidin' 3459 {
3495 3460
3496 // We only want to deactivate the PID Controller if we think we want to have our surrogate 3461 // Non-Vehicles have a limited set of Hover options.
3497 // react to the physics scene by moving it's position. 3462 // determine what our target height really is based on HoverType
3498 // Avatar to Avatar collisions
3499 // Prim to avatar collisions
3500 3463
3501 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); 3464 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(lpos.X, lpos.Y);
3502 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
3503 // ? d.BodyAddForce(Body, 0, 0, fz);
3504 return;
3505 }
3506 else
3507 {
3508 _zeroFlag = false;
3509 3465
3510 // We're flying and colliding with something 3466 switch (m_PIDHoverType)
3511 fz = ((fz - vel.Z) * (PID_D)); 3467 {
3512 } 3468 case PIDHoverType.Ground:
3513 } 3469 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3514 } 3470 break;
3515 else
3516 {
3517 float b = (1.0f - m_buoyancy);
3518 fx = _parent_scene.gravityx * b;
3519 fy = _parent_scene.gravityy * b;
3520 fz = _parent_scene.gravityz * b;
3521 }
3522 3471
3523 fx *= m_mass; 3472 case PIDHoverType.GroundAndWater:
3524 fy *= m_mass; 3473 m_waterHeight = _parent_scene.GetWaterLevel();
3525 fz *= m_mass; 3474 if (m_groundHeight > m_waterHeight)
3475 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3476 else
3477 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
3478 break;
3479 } // end switch (m_PIDHoverType)
3526 3480
3527 // constant force 3481 // don't go underground unless volumedetector
3528 fx += m_force.X;
3529 fy += m_force.Y;
3530 fz += m_force.Z;
3531 3482
3532 fx += m_forceacc.X; 3483 if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect)
3533 fy += m_forceacc.Y; 3484 {
3534 fz += m_forceacc.Z; 3485 d.Vector3 vel = d.BodyGetLinearVel(Body);
3535 3486
3536 m_forceacc = Vector3.Zero; 3487 fz = (m_targetHoverHeight - lpos.Z);
3537 3488
3538 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); 3489 // if error is zero, use position control; otherwise, velocity control
3539 if (fx != 0 || fy != 0 || fz != 0) 3490 if (Math.Abs(fz) < 0.01f)
3540 { 3491 {
3541 d.BodyAddForce(Body, fx, fy, fz); 3492 d.BodySetPosition(Body, lpos.X, lpos.Y, m_targetHoverHeight);
3542 //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); 3493 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
3494 return;
3543 } 3495 }
3496 else
3497 {
3498 _zeroFlag = false;
3499 fz /= m_PIDHoverTau;
3544 3500
3545 Vector3 trq; 3501 float tmp = Math.Abs(fz);
3502 if (tmp > 50)
3503 fz = 50 * Math.Sign(fz);
3504 else if (tmp < 0.1)
3505 fz = 0.1f * Math.Sign(fz);
3546 3506
3547 trq = _torque; 3507 fz = ((fz - vel.Z) * m_invTimeStep);
3548 trq += m_angularForceacc;
3549 m_angularForceacc = Vector3.Zero;
3550 if (trq.X != 0 || trq.Y != 0 || trq.Z != 0)
3551 {
3552 d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z);
3553 } 3508 }
3554 } 3509 }
3510 }
3511 else
3512 {
3513 float b = (1.0f - m_buoyancy);
3514 fx = _parent_scene.gravityx * b;
3515 fy = _parent_scene.gravityy * b;
3516 fz = _parent_scene.gravityz * b;
3517 }
3555 3518
3556 // update our ideia of velocities and acelerations 3519 fx *= m_mass;
3557 d.Quaternion ori; 3520 fy *= m_mass;
3558 d.Vector3 dtmpu; 3521 fz *= m_mass;
3559
3560 _position.X = lpos.X;
3561 _position.Y = lpos.Y;
3562 _position.Z = lpos.Z;
3563
3564 d.GeomCopyQuaternion(prim_geom, out ori);
3565 _orientation.X = ori.X;
3566 _orientation.Y = ori.Y;
3567 _orientation.Z = ori.Z;
3568 _orientation.W = ori.W;
3569 3522
3570 _acceleration = _velocity; 3523 // constant force
3524 fx += m_force.X;
3525 fy += m_force.Y;
3526 fz += m_force.Z;
3571 3527
3572 dtmpu = d.BodyGetLinearVel(Body); 3528 fx += m_forceacc.X;
3573 _velocity.X = dtmpu.X; 3529 fy += m_forceacc.Y;
3574 _velocity.Y = dtmpu.Y; 3530 fz += m_forceacc.Z;
3575 _velocity.Z = dtmpu.Z;
3576 3531
3577 float invts = 1 / timestep; 3532 m_forceacc = Vector3.Zero;
3578 _acceleration = (_velocity - _acceleration) * invts;
3579 3533
3580 dtmpu = d.BodyGetAngularVel(Body); 3534 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
3581 m_rotationalVelocity.X = dtmpu.X; 3535 if (fx != 0 || fy != 0 || fz != 0)
3582 m_rotationalVelocity.Y = dtmpu.Y; 3536 {
3583 m_rotationalVelocity.Z = dtmpu.Z; 3537 d.BodyAddForce(Body, fx, fy, fz);
3538 //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
3584 } 3539 }
3585 3540
3586 else // body disabled/sleeping 3541 Vector3 trq;
3587 {
3588 // let vehicles sleep
3589 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
3590 return;
3591 3542
3592 if (++bodydisablecontrol < 20) 3543 trq = _torque;
3593 return; 3544 trq += m_angularForceacc;
3594 3545 m_angularForceacc = Vector3.Zero;
3595 bodydisablecontrol = 0; 3546 if (trq.X != 0 || trq.Y != 0 || trq.Z != 0)
3596 d.BodyEnable(Body); 3547 {
3597 return; 3548 d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z);
3598 } 3549 }
3599 } 3550 }
3600 else 3551 else
@@ -3606,92 +3557,113 @@ namespace OpenSim.Region.Physics.OdePlugin
3606 } 3557 }
3607 } 3558 }
3608 3559
3609 3560 public void UpdatePositionAndVelocity()
3610 public void UpdatePositionAndVelocity(float simulatedtime)
3611 { 3561 {
3612 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! 3562 if (_parent == null && !m_disabled && !m_building && !m_outbounds && Body != IntPtr.Zero)
3613 if (_parent == null && !m_disabled && !m_building && !m_outbounds)
3614 { 3563 {
3615 if (Body != IntPtr.Zero) 3564 if (d.BodyIsEnabled(Body) || !_zeroFlag)
3616 { 3565 {
3617 bool lastZeroFlag = _zeroFlag; 3566 bool lastZeroFlag = _zeroFlag;
3618 3567
3619 if ((Math.Abs(m_lastposition.X - _position.X) < 0.01) 3568 d.Vector3 lpos;
3620 && (Math.Abs(m_lastposition.Y - _position.Y) < 0.01) 3569 d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator
3621 && (Math.Abs(m_lastposition.Z - _position.Z) < 0.01) 3570
3622 && (Math.Abs(m_lastorientation.X - _orientation.X) < 0.0001) 3571 d.Quaternion ori;
3623 && (Math.Abs(m_lastorientation.Y - _orientation.Y) < 0.0001) 3572 d.GeomCopyQuaternion(prim_geom, out ori);
3624 && (Math.Abs(m_lastorientation.Z - _orientation.Z) < 0.0001) 3573
3574 // decide if moving
3575 // use positions since this are integrated quantities
3576 // tolerance values depende a lot on simulation noise...
3577 // use simple math.abs since we dont need to be exact
3578
3579 if (
3580 (Math.Abs(_position.X - lpos.X) < 0.001f)
3581 && (Math.Abs(_position.Y - lpos.Y) < 0.001f)
3582 && (Math.Abs(_position.Z - lpos.Z) < 0.001f)
3583 && (Math.Abs(_orientation.X - ori.X) < 0.0001f)
3584 && (Math.Abs(_orientation.Y - ori.Y) < 0.0001f)
3585 && (Math.Abs(_orientation.Z - ori.Z) < 0.0001f) // ignore W
3625 ) 3586 )
3626 { 3587 {
3627 _zeroFlag = true; 3588 _zeroFlag = true;
3628 m_throttleUpdates = false;
3629 } 3589 }
3630 else 3590 else
3631 {
3632 _zeroFlag = false; 3591 _zeroFlag = false;
3633 m_lastUpdateSent = false;
3634 }
3635 3592
3636 if (_zeroFlag) 3593 // update velocities and aceleration
3594 if (!(_zeroFlag && lastZeroFlag))
3637 { 3595 {
3638 m_lastposition = _position; 3596 d.Vector3 vel = d.BodyGetLinearVel(Body);
3639 m_lastorientation = _orientation;
3640 3597
3641 _velocity = Vector3.Zero; 3598 _acceleration = _velocity;
3642 _acceleration = Vector3.Zero;
3643 m_rotationalVelocity = Vector3.Zero;
3644 3599
3645 if (!m_lastUpdateSent) 3600 if ((Math.Abs(vel.X) < 0.001f) &&
3601 (Math.Abs(vel.Y) < 0.001f) &&
3602 (Math.Abs(vel.Z) < 0.001f))
3646 { 3603 {
3647 m_throttleUpdates = false; 3604 _velocity = Vector3.Zero;
3648 throttleCounter = 0; 3605 float t = -m_invTimeStep;
3649 3606 _acceleration = _acceleration * t;
3650 base.RequestPhysicsterseUpdate();
3651
3652 m_lastUpdateSent = true;
3653 } 3607 }
3654 } 3608 else
3655 else
3656 {
3657 if (lastZeroFlag != _zeroFlag)
3658 { 3609 {
3659 base.RequestPhysicsterseUpdate(); 3610 _velocity.X = vel.X;
3611 _velocity.Y = vel.Y;
3612 _velocity.Z = vel.Z;
3613 _acceleration = (_velocity - _acceleration) * m_invTimeStep;
3660 } 3614 }
3661 3615
3662 m_lastUpdateSent = false; 3616 if ((Math.Abs(_acceleration.X) < 0.01f) &&
3663 if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) 3617 (Math.Abs(_acceleration.Y) < 0.01f) &&
3618 (Math.Abs(_acceleration.Z) < 0.01f))
3664 { 3619 {
3665 m_lastposition = _position; 3620 _acceleration = Vector3.Zero;
3666 m_lastorientation = _orientation; 3621 }
3667 m_lastVelocity = _velocity; 3622
3668 base.RequestPhysicsterseUpdate(); 3623 if ((Math.Abs(_orientation.X - ori.X) < 0.0001) &&
3624 (Math.Abs(_orientation.Y - ori.Y) < 0.0001) &&
3625 (Math.Abs(_orientation.Z - ori.Z) < 0.0001)
3626 )
3627 {
3628 m_rotationalVelocity = Vector3.Zero;
3669 } 3629 }
3670 else 3630 else
3671 { 3631 {
3672 throttleCounter++; 3632 vel = d.BodyGetAngularVel(Body);
3633 m_rotationalVelocity.X = vel.X;
3634 m_rotationalVelocity.Y = vel.Y;
3635 m_rotationalVelocity.Z = vel.Z;
3673 } 3636 }
3674 } 3637 }
3675 }
3676 else if (!m_lastUpdateSent || !_zeroFlag)
3677 {
3678 // Not a body.. so Make sure the client isn't interpolating
3679 _velocity = Vector3.Zero;
3680 _acceleration = Vector3.Zero;
3681 m_rotationalVelocity = Vector3.Zero;
3682 m_lastVelocity = Vector3.Zero;
3683
3684 _zeroFlag = true;
3685 3638
3686 if (!m_lastUpdateSent) 3639 if (_zeroFlag)
3687 { 3640 {
3688 m_throttleUpdates = false; 3641 if (lastZeroFlag)
3689 throttleCounter = 0; 3642 {
3690 3643 _velocity = Vector3.Zero;
3691 base.RequestPhysicsterseUpdate(); 3644 _acceleration = Vector3.Zero;
3645 m_rotationalVelocity = Vector3.Zero;
3646 }
3692 3647
3693 m_lastUpdateSent = true; 3648 if (!m_lastUpdateSent)
3649 {
3650 base.RequestPhysicsterseUpdate();
3651 if (lastZeroFlag)
3652 m_lastUpdateSent = true;
3653 }
3654 return;
3694 } 3655 }
3656
3657 _position.X = lpos.X;
3658 _position.Y = lpos.Y;
3659 _position.Z = lpos.Z;
3660
3661 _orientation.X = ori.X;
3662 _orientation.Y = ori.Y;
3663 _orientation.Z = ori.Z;
3664 _orientation.W = ori.W;
3665 base.RequestPhysicsterseUpdate();
3666 m_lastUpdateSent = false;
3695 } 3667 }
3696 } 3668 }
3697 } 3669 }
@@ -3849,11 +3821,11 @@ namespace OpenSim.Region.Physics.OdePlugin
3849 break; 3821 break;
3850 3822
3851 case changes.AddForce: 3823 case changes.AddForce:
3852 changeAddForce((Vector3)arg); 3824 changeAddImpulse((Vector3)arg);
3853 break; 3825 break;
3854 3826
3855 case changes.AddAngForce: 3827 case changes.AddAngForce:
3856 changeAddAngularForce((Vector3)arg); 3828 changeAddAngularImpulse((Vector3)arg);
3857 break; 3829 break;
3858 3830
3859 case changes.AngLock: 3831 case changes.AngLock:
generated by cgit v1.1 at 2024-12-18 18:16:41 +0000