aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/ChOdePlugin
diff options
context:
space:
mode:
authorKitto Flora2010-03-14 16:22:13 -0400
committerKitto Flora2010-03-14 16:22:13 -0400
commit819806261026cccd68dee649f11938ae5bf10029 (patch)
tree4e772b2c06f42ace9236223da704666900ccae3e /OpenSim/Region/Physics/ChOdePlugin
parentFix Rotation Lock. NOTE: This version had poor border crossing control (diff)
downloadopensim-SC_OLD-819806261026cccd68dee649f11938ae5bf10029.zip
opensim-SC_OLD-819806261026cccd68dee649f11938ae5bf10029.tar.gz
opensim-SC_OLD-819806261026cccd68dee649f11938ae5bf10029.tar.bz2
opensim-SC_OLD-819806261026cccd68dee649f11938ae5bf10029.tar.xz
RotLookAt repaired; debug msg cleanup.
Diffstat (limited to 'OpenSim/Region/Physics/ChOdePlugin')
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs552
1 files changed, 144 insertions, 408 deletions
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
index 367927d..5e6696e 100644
--- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
@@ -652,7 +652,7 @@ namespace OpenSim.Region.Physics.OdePlugin
652 return true; 652 return true;
653 } 653 }
654 654
655 public override Vector3 Acceleration 655 public override Vector3 Acceleration // client updates read data via here
656 { 656 {
657 get { return _acceleration; } 657 get { return _acceleration; }
658 } 658 }
@@ -696,14 +696,6 @@ namespace OpenSim.Region.Physics.OdePlugin
696 { 696 {
697 get 697 get
698 { 698 {
699/* Vector3 pv = Vector3.Zero;
700 if (_zeroFlag)
701 return pv;
702 m_lastUpdateSent = false;
703
704 if (m_rotationalVelocity.ApproxEquals(pv, 0.2f))
705 return pv;
706*/
707 return m_rotationalVelocity; 699 return m_rotationalVelocity;
708 } 700 }
709 set 701 set
@@ -1398,9 +1390,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1398 if (m_taintCollidesWater != m_collidesWater) 1390 if (m_taintCollidesWater != m_collidesWater)
1399 changefloatonwater(timestep); 1391 changefloatonwater(timestep);
1400 1392
1401 // ##*
1402 if (!m_angularEnable.ApproxEquals(m_taintAngularLock,0f)) 1393 if (!m_angularEnable.ApproxEquals(m_taintAngularLock,0f))
1403Console.WriteLine("ALchange req {0} is {1}", m_taintAngularLock, m_angularEnable);
1404 changeAngularLock(timestep); 1394 changeAngularLock(timestep);
1405 1395
1406 } 1396 }
@@ -1411,17 +1401,11 @@ Console.WriteLine("ALchange req {0} is {1}", m_taintAngularLock, m_angularEna
1411 } 1401 }
1412 1402
1413 1403
1414 private void changeAngularLock(float timestep) // ##* 1404 private void changeAngularLock(float timestep)
1415 { 1405 {
1416 // do we have a Physical object? 1406 if (_parent == null)
1417// if (Body != IntPtr.Zero) 1407 {
1418// { 1408 m_angularEnable = m_taintAngularLock;
1419 //Check that we have a Parent
1420 //If we have a parent then we're not authorative here
1421 if (_parent == null)
1422 {
1423Console.WriteLine("Alock changed to {0}", m_taintAngularLock);
1424 m_angularEnable = m_taintAngularLock;
1425 } 1409 }
1426 } 1410 }
1427 1411
@@ -1435,7 +1419,6 @@ Console.WriteLine("Alock changed to {0}", m_taintAngularLock);
1435 { 1419 {
1436 OdePrim obj = (OdePrim)m_taintparent; 1420 OdePrim obj = (OdePrim)m_taintparent;
1437 //obj.disableBody(); 1421 //obj.disableBody();
1438//Console.WriteLine("changelink calls ParentPrim");
1439 obj.ParentPrim(this); 1422 obj.ParentPrim(this);
1440 1423
1441 /* 1424 /*
@@ -1453,8 +1436,6 @@ Console.WriteLine("Alock changed to {0}", m_taintAngularLock);
1453 // destroy link 1436 // destroy link
1454 else if (_parent != null && m_taintparent == null) 1437 else if (_parent != null && m_taintparent == null)
1455 { 1438 {
1456//Console.WriteLine(" changelink B");
1457
1458 if (_parent is OdePrim) 1439 if (_parent is OdePrim)
1459 { 1440 {
1460 OdePrim obj = (OdePrim)_parent; 1441 OdePrim obj = (OdePrim)_parent;
@@ -1480,7 +1461,6 @@ Console.WriteLine("Alock changed to {0}", m_taintAngularLock);
1480 // prim is the child 1461 // prim is the child
1481 public void ParentPrim(OdePrim prim) 1462 public void ParentPrim(OdePrim prim)
1482 { 1463 {
1483//Console.WriteLine("ParentPrim " + m_primName);
1484 if (this.m_localID != prim.m_localID) 1464 if (this.m_localID != prim.m_localID)
1485 { 1465 {
1486 if (Body == IntPtr.Zero) 1466 if (Body == IntPtr.Zero)
@@ -1494,7 +1474,6 @@ Console.WriteLine("Alock changed to {0}", m_taintAngularLock);
1494 { 1474 {
1495 if (!childrenPrim.Contains(prim)) 1475 if (!childrenPrim.Contains(prim))
1496 { 1476 {
1497//Console.WriteLine("childrenPrim.Add " + prim);
1498 childrenPrim.Add(prim); 1477 childrenPrim.Add(prim);
1499 1478
1500 foreach (OdePrim prm in childrenPrim) 1479 foreach (OdePrim prm in childrenPrim)
@@ -1640,7 +1619,6 @@ Console.WriteLine("Alock changed to {0}", m_taintAngularLock);
1640 { 1619 {
1641 foreach (OdePrim prm in childrenPrim) 1620 foreach (OdePrim prm in childrenPrim)
1642 { 1621 {
1643//Console.WriteLine("ChildSetGeom calls ParentPrim");
1644 ParentPrim(prm); 1622 ParentPrim(prm);
1645 } 1623 }
1646 } 1624 }
@@ -1667,7 +1645,6 @@ Console.WriteLine("Alock changed to {0}", m_taintAngularLock);
1667 1645
1668 lock (childrenPrim) 1646 lock (childrenPrim)
1669 { 1647 {
1670 //Console.WriteLine("childrenPrim.Remove " + odePrim);
1671 childrenPrim.Remove(odePrim); 1648 childrenPrim.Remove(odePrim);
1672 } 1649 }
1673 1650
@@ -1680,7 +1657,6 @@ Console.WriteLine("Alock changed to {0}", m_taintAngularLock);
1680 { 1657 {
1681 foreach (OdePrim prm in childrenPrim) 1658 foreach (OdePrim prm in childrenPrim)
1682 { 1659 {
1683//Console.WriteLine("ChildDelink calls ParentPrim");
1684 ParentPrim(prm); 1660 ParentPrim(prm);
1685 } 1661 }
1686 } 1662 }
@@ -2417,219 +2393,9 @@ Console.WriteLine(" JointCreateFixed");
2417 2393
2418 public void UpdatePositionAndVelocity() 2394 public void UpdatePositionAndVelocity()
2419 { 2395 {
2420 return; // moved to the MOVE() method 2396 return; // moved to the Move() method
2421 } 2397 }
2422 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! 2398/* No one uses this?
2423/* if (_parent == null)
2424 {
2425 Vector3 pv = Vector3.Zero;
2426 bool lastZeroFlag = _zeroFlag;
2427 if (Body != (IntPtr)0) // FIXME -> or if it is a joint
2428 {
2429 d.Vector3 vec = d.BodyGetPosition(Body);
2430 d.Quaternion ori = d.BodyGetQuaternion(Body);
2431 d.Vector3 vel = d.BodyGetLinearVel(Body);
2432 d.Vector3 rotvel = d.BodyGetAngularVel(Body);
2433 d.Vector3 torque = d.BodyGetTorque(Body);
2434 _torque = new Vector3(torque.X, torque.Y, torque.Z);
2435 Vector3 l_position = Vector3.Zero;
2436 Quaternion l_orientation = Quaternion.Identity;
2437
2438 // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!)
2439 //if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
2440 //if (vec.Y < 0.0f) { vec.Y = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
2441 //if (vec.X > 255.95f) { vec.X = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
2442 //if (vec.Y > 255.95f) { vec.Y = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
2443
2444 m_lastposition = _position;
2445 m_lastorientation = _orientation;
2446
2447 l_position.X = vec.X;
2448 l_position.Y = vec.Y;
2449 l_position.Z = vec.Z;
2450 l_orientation.X = ori.X;
2451 l_orientation.Y = ori.Y;
2452 l_orientation.Z = ori.Z;
2453 l_orientation.W = ori.W;
2454
2455// if(l_position.Y != m_lastposition.Y){
2456// Console.WriteLine("UP&V {0} {1}", m_primName, l_position);
2457// }
2458
2459 if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || l_position.X < 0f || l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || l_position.Y < 0f)
2460 {
2461 //base.RaiseOutOfBounds(l_position);
2462
2463 if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)
2464 {
2465 _position = l_position;
2466 //_parent_scene.remActivePrim(this);
2467 if (_parent == null)
2468 base.RequestPhysicsterseUpdate();
2469 return;
2470 }
2471 else
2472 {
2473 if (_parent == null)
2474 base.RaiseOutOfBounds(l_position);
2475 return;
2476 }
2477 }
2478
2479 if (l_position.Z < 0)
2480 {
2481 // This is so prim that get lost underground don't fall forever and suck up
2482 //
2483 // Sim resources and memory.
2484 // Disables the prim's movement physics....
2485 // It's a hack and will generate a console message if it fails.
2486
2487 //IsPhysical = false;
2488 if (_parent == null)
2489 base.RaiseOutOfBounds(_position);
2490
2491 _acceleration.X = 0;
2492 _acceleration.Y = 0;
2493 _acceleration.Z = 0;
2494
2495 _velocity.X = 0;
2496 _velocity.Y = 0;
2497 _velocity.Z = 0;
2498 m_rotationalVelocity.X = 0;
2499 m_rotationalVelocity.Y = 0;
2500 m_rotationalVelocity.Z = 0;
2501
2502 if (_parent == null)
2503 base.RequestPhysicsterseUpdate();
2504
2505 m_throttleUpdates = false;
2506 throttleCounter = 0;
2507 _zeroFlag = true;
2508 //outofBounds = true;
2509 }
2510
2511 //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation));
2512//Console.WriteLine("Adiff " + m_primName + " = " + Adiff);
2513 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
2514 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
2515 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
2516// && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01))
2517 && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large
2518 {
2519 _zeroFlag = true;
2520//Console.WriteLine("ZFT 2");
2521 m_throttleUpdates = false;
2522 }
2523 else
2524 {
2525 //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
2526 _zeroFlag = false;
2527 m_lastUpdateSent = false;
2528 //m_throttleUpdates = false;
2529 }
2530
2531 if (_zeroFlag)
2532 {
2533 _velocity.X = 0.0f;
2534 _velocity.Y = 0.0f;
2535 _velocity.Z = 0.0f;
2536
2537 _acceleration.X = 0;
2538 _acceleration.Y = 0;
2539 _acceleration.Z = 0;
2540
2541 //_orientation.w = 0f;
2542 //_orientation.X = 0f;
2543 //_orientation.Y = 0f;
2544 //_orientation.Z = 0f;
2545 m_rotationalVelocity.X = 0;
2546 m_rotationalVelocity.Y = 0;
2547 m_rotationalVelocity.Z = 0;
2548 if (!m_lastUpdateSent)
2549 {
2550 m_throttleUpdates = false;
2551 throttleCounter = 0;
2552 m_rotationalVelocity = pv;
2553
2554 if (_parent == null)
2555 {
2556 base.RequestPhysicsterseUpdate();
2557 }
2558
2559 m_lastUpdateSent = true;
2560 }
2561 }
2562 else
2563 {
2564 if (lastZeroFlag != _zeroFlag)
2565 {
2566 if (_parent == null)
2567 {
2568 base.RequestPhysicsterseUpdate();
2569 }
2570 }
2571
2572 m_lastVelocity = _velocity;
2573
2574 _position = l_position;
2575
2576 _velocity.X = vel.X;
2577 _velocity.Y = vel.Y;
2578 _velocity.Z = vel.Z;
2579
2580 _acceleration = ((_velocity - m_lastVelocity) / 0.1f);
2581 _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f);
2582 //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString());
2583
2584// if (_velocity.ApproxEquals(pv, 0.5f)) ???? Disregard rotational vel if lin vel is < 0.5 ?????
2585// {
2586// m_rotationalVelocity = pv;/
2587
2588// }
2589// else
2590// {
2591 m_rotationalVelocity = new Vector3(rotvel.X, rotvel.Y, rotvel.Z);
2592// }
2593
2594 //m_log.Debug("ODE: " + m_rotationalVelocity.ToString());
2595 _orientation.X = ori.X;
2596 _orientation.Y = ori.Y;
2597 _orientation.Z = ori.Z;
2598 _orientation.W = ori.W;
2599 m_lastUpdateSent = false;
2600 if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
2601 {
2602 if (_parent == null)
2603 {
2604 base.RequestPhysicsterseUpdate();
2605 }
2606 }
2607 else
2608 {
2609 throttleCounter++;
2610 }
2611 }
2612 m_lastposition = l_position;
2613 }
2614 else
2615 {
2616 // Not a body.. so Make sure the client isn't interpolating
2617 _velocity.X = 0;
2618 _velocity.Y = 0;
2619 _velocity.Z = 0;
2620
2621 _acceleration.X = 0;
2622 _acceleration.Y = 0;
2623 _acceleration.Z = 0;
2624
2625 m_rotationalVelocity.X = 0;
2626 m_rotationalVelocity.Y = 0;
2627 m_rotationalVelocity.Z = 0;
2628 _zeroFlag = true;
2629 }
2630 }
2631 }
2632*/
2633 public Matrix4 FromDMass(d.Mass pMass) 2399 public Matrix4 FromDMass(d.Mass pMass)
2634 { 2400 {
2635 Matrix4 obj; 2401 Matrix4 obj;
@@ -2651,7 +2417,7 @@ Console.WriteLine(" JointCreateFixed");
2651 obj.M44 = 1; 2417 obj.M44 = 1;
2652 return obj; 2418 return obj;
2653 } 2419 }
2654 2420*/
2655 public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj) 2421 public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj)
2656 { 2422 {
2657 obj.I.M00 = pMat[0, 0]; 2423 obj.I.M00 = pMat[0, 0];
@@ -2878,11 +2644,6 @@ Console.WriteLine(" JointCreateFixed");
2878 m_material = pMaterial; 2644 m_material = pMaterial;
2879 } 2645 }
2880 2646
2881
2882
2883
2884
2885
2886 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) 2647 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
2887 { 2648 {
2888 switch (pParam) 2649 switch (pParam)
@@ -3242,193 +3003,173 @@ Console.WriteLine(" JointCreateFixed");
3242 { 3003 {
3243 3004
3244// Old public void UpdatePositionAndVelocity(), more accuratley calculated here 3005// Old public void UpdatePositionAndVelocity(), more accuratley calculated here
3245 // Vector3 pv = Vector3.Zero; // what was this for? 3006 bool lastZeroFlag = _zeroFlag; // was it stopped
3246 bool lastZeroFlag = _zeroFlag; // was it stopped 3007 d.Vector3 vec = d.BodyGetPosition(Body);
3247 // if (Body != (IntPtr)0) // FIXME -> or if it is a joint 3008 d.Quaternion ori = d.BodyGetQuaternion(Body);
3248 // { 3009 d.Vector3 vel = d.BodyGetLinearVel(Body);
3249 d.Vector3 vec = d.BodyGetPosition(Body);
3250 d.Quaternion ori = d.BodyGetQuaternion(Body);
3251 d.Vector3 vel = d.BodyGetLinearVel(Body);
3252 // d.Vector3 rotvel = d.BodyGetAngularVel(Body); 3010 // d.Vector3 rotvel = d.BodyGetAngularVel(Body);
3253 d.Vector3 torque = d.BodyGetTorque(Body); 3011 d.Vector3 torque = d.BodyGetTorque(Body);
3254 _torque = new Vector3(torque.X, torque.Y, torque.Z); 3012 _torque = new Vector3(torque.X, torque.Y, torque.Z);
3255 Vector3 l_position = Vector3.Zero; 3013 Vector3 l_position = Vector3.Zero;
3256 Quaternion l_orientation = Quaternion.Identity; 3014 Quaternion l_orientation = Quaternion.Identity;
3257 3015
3258 // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!) 3016 m_lastposition = _position;
3259 //if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } 3017 m_lastorientation = _orientation;
3260 //if (vec.Y < 0.0f) { vec.Y = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
3261 //if (vec.X > 255.95f) { vec.X = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
3262 //if (vec.Y > 255.95f) { vec.Y = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
3263
3264 m_lastposition = _position;
3265 m_lastorientation = _orientation;
3266 3018
3267 l_position.X = vec.X; 3019 l_position.X = vec.X;
3268 l_position.Y = vec.Y; 3020 l_position.Y = vec.Y;
3269 l_position.Z = vec.Z; 3021 l_position.Z = vec.Z;
3270 l_orientation.X = ori.X; 3022 l_orientation.X = ori.X;
3271 l_orientation.Y = ori.Y; 3023 l_orientation.Y = ori.Y;
3272 l_orientation.Z = ori.Z; 3024 l_orientation.Z = ori.Z;
3273 l_orientation.W = ori.W; 3025 l_orientation.W = ori.W;
3274//Console.WriteLine("Move {0} at {1}", m_primName, l_position); 3026//Console.WriteLine("Move {0} at {1}", m_primName, l_position);
3275 3027
3028 // Check if outside region horizontally
3029 if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) ||
3030 l_position.X < 0f ||
3031 l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) ||
3032 l_position.Y < 0f)
3033 {
3034 if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)
3035 { // keep trying to cross?
3036 _position = l_position;
3037 //_parent_scene.remActivePrim(this);
3038 if (_parent == null) base.RequestPhysicsterseUpdate();
3039 return; // Dont process any other motion?
3040 }
3041 else
3042 { // Too many tries
3043 if (_parent == null) base.RaiseOutOfBounds(l_position);
3044 return; // Dont process any other motion?
3045 }
3046 } // end outside region horizontally
3276 3047
3277 if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || 3048 if (l_position.Z < 0)
3278 l_position.X < 0f || 3049 {
3279 l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || 3050 // This is so prim that get lost underground don't fall forever and suck up
3280 l_position.Y < 0f) 3051 //
3281 { 3052 // Sim resources and memory.
3282 //base.RaiseOutOfBounds(l_position); 3053 // Disables the prim's movement physics....
3054 // It's a hack and will generate a console message if it fails.
3283 3055
3284 if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds) 3056 //IsPhysical = false;
3285 { 3057 if (_parent == null) base.RaiseOutOfBounds(_position);
3286 _position = l_position; 3058
3287 //_parent_scene.remActivePrim(this); 3059 _acceleration.X = 0; // This stuff may stop client display but it has no
3288 if (_parent == null) 3060 _acceleration.Y = 0; // effect on the object in phys engine!
3289 base.RequestPhysicsterseUpdate(); 3061 _acceleration.Z = 0;
3290 return;
3291 }
3292 else
3293 {
3294 if (_parent == null)
3295 base.RaiseOutOfBounds(l_position);
3296 return;
3297 }
3298 }
3299 3062
3300 if (l_position.Z < 0) 3063 _velocity.X = 0;
3301 { 3064 _velocity.Y = 0;
3302 // This is so prim that get lost underground don't fall forever and suck up 3065 _velocity.Z = 0;
3303 // 3066 m_rotationalVelocity.X = 0;
3304 // Sim resources and memory. 3067 m_rotationalVelocity.Y = 0;
3305 // Disables the prim's movement physics.... 3068 m_rotationalVelocity.Z = 0;
3306 // It's a hack and will generate a console message if it fails.
3307 3069
3308 //IsPhysical = false; 3070 if (_parent == null) base.RequestPhysicsterseUpdate();
3309 if (_parent == null)
3310 base.RaiseOutOfBounds(_position);
3311 3071
3312 _acceleration.X = 0; 3072 m_throttleUpdates = false;
3313 _acceleration.Y = 0; 3073 throttleCounter = 0;
3314 _acceleration.Z = 0; 3074 _zeroFlag = true;
3075 //outofBounds = true;
3076 } // end neg Z check
3315 3077
3316 _velocity.X = 0; 3078 // Is it moving?
3317 _velocity.Y = 0; 3079 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
3318 _velocity.Z = 0; 3080 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
3319 m_rotationalVelocity.X = 0; 3081 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
3320 m_rotationalVelocity.Y = 0; 3082 && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large
3321 m_rotationalVelocity.Z = 0; 3083 {
3084 _zeroFlag = true;
3085//Console.WriteLine("ZFT 2");
3086 m_throttleUpdates = false;
3087 }
3088 else
3089 {
3090 //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
3091 _zeroFlag = false;
3092 m_lastUpdateSent = false;
3093 //m_throttleUpdates = false;
3094 }
3322 3095
3323 if (_parent == null) 3096 if (_zeroFlag)
3324 base.RequestPhysicsterseUpdate(); 3097 { // Its stopped
3098 _velocity.X = 0.0f;
3099 _velocity.Y = 0.0f;
3100 _velocity.Z = 0.0f;
3325 3101
3102 _acceleration.X = 0;
3103 _acceleration.Y = 0;
3104 _acceleration.Z = 0;
3105 //_orientation.w = 0f;
3106 //_orientation.X = 0f;
3107 //_orientation.Y = 0f;
3108 //_orientation.Z = 0f;
3109 m_rotationalVelocity.X = 0;
3110 m_rotationalVelocity.Y = 0;
3111 m_rotationalVelocity.Z = 0;
3112 if (!m_lastUpdateSent)
3113 {
3326 m_throttleUpdates = false; 3114 m_throttleUpdates = false;
3327 throttleCounter = 0; 3115 throttleCounter = 0;
3328 _zeroFlag = true; 3116 if (_parent == null)
3329 //outofBounds = true; 3117 {
3330 } 3118 base.RequestPhysicsterseUpdate();
3119 }
3331 3120
3332 //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)); 3121 m_lastUpdateSent = true;
3333//Console.WriteLine("Adiff " + m_primName + " = " + Adiff);
3334 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
3335 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
3336 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
3337// && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01))
3338 && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large
3339 {
3340 _zeroFlag = true;
3341//Console.WriteLine("ZFT 2");
3342 m_throttleUpdates = false;
3343 } 3122 }
3344 else 3123 }
3124 else
3125 { // Its moving
3126 if (lastZeroFlag != _zeroFlag)
3345 { 3127 {
3346 //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString()); 3128 if (_parent == null)
3347 _zeroFlag = false;
3348 m_lastUpdateSent = false;
3349 //m_throttleUpdates = false;
3350 }
3351
3352 if (_zeroFlag)
3353 { // Its stopped
3354 _velocity.X = 0.0f;
3355 _velocity.Y = 0.0f;
3356 _velocity.Z = 0.0f;
3357
3358 _acceleration.X = 0;
3359 _acceleration.Y = 0;
3360 _acceleration.Z = 0;
3361
3362 //_orientation.w = 0f;
3363 //_orientation.X = 0f;
3364 //_orientation.Y = 0f;
3365 //_orientation.Z = 0f;
3366 m_rotationalVelocity.X = 0;
3367 m_rotationalVelocity.Y = 0;
3368 m_rotationalVelocity.Z = 0;
3369 if (!m_lastUpdateSent)
3370 { 3129 {
3371 m_throttleUpdates = false; 3130 base.RequestPhysicsterseUpdate();
3372 throttleCounter = 0;
3373 // m_rotationalVelocity = pv; What was this for?
3374
3375 if (_parent == null)
3376 {
3377 base.RequestPhysicsterseUpdate();
3378 }
3379
3380 m_lastUpdateSent = true;
3381 } 3131 }
3382 } 3132 }
3383 else
3384 { // Its moving
3385 if (lastZeroFlag != _zeroFlag)
3386 {
3387 if (_parent == null)
3388 {
3389 base.RequestPhysicsterseUpdate();
3390 }
3391 }
3392 3133
3393 m_lastVelocity = _velocity; 3134 m_lastVelocity = _velocity;
3394 3135
3395 _position = l_position; 3136 _position = l_position;
3396 3137
3397 _velocity.X = vel.X; 3138 _velocity.X = vel.X;
3398 _velocity.Y = vel.Y; 3139 _velocity.Y = vel.Y;
3399 _velocity.Z = vel.Z; 3140 _velocity.Z = vel.Z;
3400// Why 2 calcs??? 3141// Why 2 calcs???
3401// _acceleration = ((_velocity - m_lastVelocity) / 0.1f); 3142// _acceleration = ((_velocity - m_lastVelocity) / 0.1f);
3402// _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, 3143// _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f,
3403// _velocity.Y - m_lastVelocity.Y / 0.1f, 3144// _velocity.Y - m_lastVelocity.Y / 0.1f,
3404// _velocity.Z - m_lastVelocity.Z / 0.1f); 3145// _velocity.Z - m_lastVelocity.Z / 0.1f);
3405 3146
3406 _acceleration = ((_velocity - m_lastVelocity) / timestep); 3147 _acceleration = ((_velocity - m_lastVelocity) / timestep);
3407 3148
3408 _orientation.X = ori.X; 3149 _orientation.X = ori.X;
3409 _orientation.Y = ori.Y; 3150 _orientation.Y = ori.Y;
3410 _orientation.Z = ori.Z; 3151 _orientation.Z = ori.Z;
3411 _orientation.W = ori.W; 3152 _orientation.W = ori.W;
3412 m_lastUpdateSent = false; 3153 m_lastUpdateSent = false;
3413 if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) 3154 if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
3414 { 3155 {
3415 if (_parent == null) 3156 if (_parent == null)
3416 {
3417 base.RequestPhysicsterseUpdate();
3418 }
3419 }
3420 else
3421 { 3157 {
3422 throttleCounter++; 3158 base.RequestPhysicsterseUpdate();
3423 } 3159 }
3424 } 3160 }
3425 m_lastposition = l_position; 3161 else
3426 3162 {
3163 throttleCounter++;
3164 }
3165 }
3166 m_lastposition = l_position;
3167
3427 /// End of old UpdatePositionAndVelocity insert 3168 /// End of old UpdatePositionAndVelocity insert
3428 3169
3429//if (!Acceleration.ApproxEquals(Vector3.Zero, 0.01f)) Console.WriteLine("Move " + m_primName + " Accel=" + Acceleration); 3170//if (!Acceleration.ApproxEquals(Vector3.Zero, 0.01f)) Console.WriteLine("Move " + m_primName + " Accel=" + Acceleration);
3430//if(frcount == 0) Console.WriteLine("Move " + m_primName + " VTyp " + m_type + 3171// if(frcount == 0) Console.WriteLine("Move " + m_primName + " VTyp " + m_type +
3431 // " usePID=" + m_usePID + " seHover=" + m_useHoverPID + " useAPID=" + m_useAPID); 3172// " usePID=" + m_usePID + " seHover=" + m_useHoverPID + " useAPID=" + m_useAPID);
3432 if (m_type != Vehicle.TYPE_NONE) 3173 if (m_type != Vehicle.TYPE_NONE)
3433 { 3174 {
3434 // get body attitude 3175 // get body attitude
@@ -3701,7 +3442,6 @@ Console.WriteLine(" JointCreateFixed");
3701//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel); 3442//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel);
3702 3443
3703 m_lastAngularVelocity = angObjectVel; 3444 m_lastAngularVelocity = angObjectVel;
3704if(frcount == 0) Console.WriteLine("angularEnable {0}", m_angularEnable);
3705 3445
3706 if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f)) 3446 if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f))
3707 { 3447 {
@@ -3720,14 +3460,11 @@ if(frcount == 0) Console.WriteLine("angularEnable {0}", m_angularEnable);
3720 d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z); 3460 d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
3721//if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity); 3461//if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity);
3722 3462
3723 } // end VEHICLES #### 3463 } // end VEHICLES
3724 else 3464 else
3725 { 3465 {
3726 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 3466 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
3727 // NON-'VEHICLES' are dealt with here 3467 // NON-'VEHICLES' are dealt with here
3728 // m_angularEnable = <1,1,1> means no lock. a 0 on axis means locked.
3729
3730// NB this may be wrong - may lock global axis! Should be LOCAL axis!
3731 /// Dynamics Angular Lock ======================================================================== 3468 /// Dynamics Angular Lock ========================================================================
3732 if (d.BodyIsEnabled(Body) && !m_angularEnable.ApproxEquals(Vector3.One, 0.003f)) 3469 if (d.BodyIsEnabled(Body) && !m_angularEnable.ApproxEquals(Vector3.One, 0.003f))
3733 { 3470 {
@@ -3802,7 +3539,7 @@ if(frcount == 0) Console.WriteLine("angularEnable {0}", m_angularEnable);
3802 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); 3539 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3803 d.BodySetLinearVel(Body, 0, 0, 0); 3540 d.BodySetLinearVel(Body, 0, 0, 0);
3804 d.BodyAddForce(Body, 0, 0, fz); 3541 d.BodyAddForce(Body, 0, 0, fz);
3805 return; 3542 // return;
3806 } 3543 }
3807 else 3544 else
3808 { 3545 {
@@ -3911,7 +3648,6 @@ if(frcount == 0) Console.WriteLine("angularEnable {0}", m_angularEnable);
3911 // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly 3648 // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly
3912 // Also in SL the mass of the object has no effect on time to get there. 3649 // Also in SL the mass of the object has no effect on time to get there.
3913 // Factors: 3650 // Factors:
3914//if(frcount == 0) Console.WriteLine("APID ");
3915 // get present body rotation 3651 // get present body rotation
3916 float limit = 1.0f; 3652 float limit = 1.0f;
3917 float scaler = 50f; // adjusts damping time 3653 float scaler = 50f; // adjusts damping time
generated by cgit v1.1 at 2024-11-10 14:31:13 +0000