aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics/OdePlugin/ODEPrim.cs')
-rw-r--r--OpenSim/Region/Physics/OdePlugin/ODEPrim.cs562
1 files changed, 282 insertions, 280 deletions
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index 3eb3b28..973aa84 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -1,5 +1,7 @@
1/* Copyright (c) Contributors, http://opensimulator.org/ 1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
2 * See CONTRIBUTORS.TXT for a full list of copyright holders. 3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
3 * Redistribution and use in source and binary forms, with or without 5 * Redistribution and use in source and binary forms, with or without
4 * modification, are permitted provided that the following conditions are met: 6 * modification, are permitted provided that the following conditions are met:
5 * * Redistributions of source code must retain the above copyright 7 * * Redistributions of source code must retain the above copyright
@@ -93,7 +95,7 @@ namespace OpenSim.Region.Physics.OdePlugin
93 private float m_targetHoverHeight; 95 private float m_targetHoverHeight;
94 private float m_groundHeight; 96 private float m_groundHeight;
95 private float m_waterHeight; 97 private float m_waterHeight;
96 private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. 98 private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
97 99
98 // private float m_tensor = 5f; 100 // private float m_tensor = 5f;
99 private int body_autodisable_frames = 20; 101 private int body_autodisable_frames = 20;
@@ -294,7 +296,7 @@ namespace OpenSim.Region.Physics.OdePlugin
294 m_taintselected = value; 296 m_taintselected = value;
295 m_isSelected = value; 297 m_isSelected = value;
296 } 298 }
297 if(m_isSelected) disableBodySoft(); 299 if (m_isSelected) disableBodySoft();
298 } 300 }
299 } 301 }
300 302
@@ -302,7 +304,7 @@ namespace OpenSim.Region.Physics.OdePlugin
302 { 304 {
303 prev_geom = prim_geom; 305 prev_geom = prim_geom;
304 prim_geom = geom; 306 prim_geom = geom;
305//Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName); 307//Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName);
306 if (prim_geom != IntPtr.Zero) 308 if (prim_geom != IntPtr.Zero)
307 { 309 {
308 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 310 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
@@ -314,7 +316,7 @@ namespace OpenSim.Region.Physics.OdePlugin
314 if (_parent != null && _parent is OdePrim) 316 if (_parent != null && _parent is OdePrim)
315 { 317 {
316 OdePrim parent = (OdePrim)_parent; 318 OdePrim parent = (OdePrim)_parent;
317//Console.WriteLine("SetGeom calls ChildSetGeom"); 319//Console.WriteLine("SetGeom calls ChildSetGeom");
318 parent.ChildSetGeom(this); 320 parent.ChildSetGeom(this);
319 } 321 }
320 } 322 }
@@ -331,7 +333,7 @@ namespace OpenSim.Region.Physics.OdePlugin
331 { 333 {
332 d.BodyEnable(Body); 334 d.BodyEnable(Body);
333 if (m_vehicle.Type != Vehicle.TYPE_NONE) 335 if (m_vehicle.Type != Vehicle.TYPE_NONE)
334 m_vehicle.Enable(Body, _parent_scene); 336 m_vehicle.Enable(Body, _parent_scene);
335 } 337 }
336 338
337 m_disabled = false; 339 m_disabled = false;
@@ -376,7 +378,7 @@ namespace OpenSim.Region.Physics.OdePlugin
376 d.BodySetAutoDisableSteps(Body, body_autodisable_frames); 378 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
377 379
378 // disconnect from world gravity so we can apply buoyancy 380 // disconnect from world gravity so we can apply buoyancy
379 d.BodySetGravityMode (Body, false); 381 d.BodySetGravityMode (Body, false);
380 382
381 m_interpenetrationcount = 0; 383 m_interpenetrationcount = 0;
382 m_collisionscore = 0; 384 m_collisionscore = 0;
@@ -872,7 +874,7 @@ namespace OpenSim.Region.Physics.OdePlugin
872 874
873 public void ProcessTaints(float timestep) 875 public void ProcessTaints(float timestep)
874 { 876 {
875//Console.WriteLine("ProcessTaints for " + m_primName ); 877//Console.WriteLine("ProcessTaints for " + m_primName);
876 if (m_taintadd) 878 if (m_taintadd)
877 { 879 {
878 changeadd(timestep); 880 changeadd(timestep);
@@ -880,24 +882,24 @@ namespace OpenSim.Region.Physics.OdePlugin
880 882
881 if (prim_geom != IntPtr.Zero) 883 if (prim_geom != IntPtr.Zero)
882 { 884 {
883 if (!_position.ApproxEquals(m_taintposition, 0f)) 885 if (!_position.ApproxEquals(m_taintposition, 0f))
884 changemove(timestep); 886 changemove(timestep);
885 887
886 if (m_taintrot != _orientation) 888 if (m_taintrot != _orientation)
887 { 889 {
888 if(childPrim && IsPhysical) // For physical child prim... 890 if (childPrim && IsPhysical) // For physical child prim...
889 { 891 {
890 rotate(timestep); 892 rotate(timestep);
891 // KF: ODE will also rotate the parent prim! 893 // KF: ODE will also rotate the parent prim!
892 // so rotate the root back to where it was 894 // so rotate the root back to where it was
893 OdePrim parent = (OdePrim)_parent; 895 OdePrim parent = (OdePrim)_parent;
894 parent.rotate(timestep); 896 parent.rotate(timestep);
895 } 897 }
896 else 898 else
897 { 899 {
898 //Just rotate the prim 900 //Just rotate the prim
899 rotate(timestep); 901 rotate(timestep);
900 } 902 }
901 } 903 }
902 // 904 //
903 905
@@ -1006,7 +1008,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1006 // destroy link 1008 // destroy link
1007 else if (_parent != null && m_taintparent == null) 1009 else if (_parent != null && m_taintparent == null)
1008 { 1010 {
1009//Console.WriteLine(" changelink B"); 1011//Console.WriteLine(" changelink B");
1010 1012
1011 if (_parent is OdePrim) 1013 if (_parent is OdePrim)
1012 { 1014 {
@@ -1033,7 +1035,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1033 // prim is the child 1035 // prim is the child
1034 public void ParentPrim(OdePrim prim) 1036 public void ParentPrim(OdePrim prim)
1035 { 1037 {
1036//Console.WriteLine("ParentPrim " + m_primName); 1038//Console.WriteLine("ParentPrim " + m_primName);
1037 if (this.m_localID != prim.m_localID) 1039 if (this.m_localID != prim.m_localID)
1038 { 1040 {
1039 if (Body == IntPtr.Zero) 1041 if (Body == IntPtr.Zero)
@@ -1047,7 +1049,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1047 { 1049 {
1048 if (!childrenPrim.Contains(prim)) 1050 if (!childrenPrim.Contains(prim))
1049 { 1051 {
1050//Console.WriteLine("childrenPrim.Add " + prim); 1052//Console.WriteLine("childrenPrim.Add " + prim);
1051 childrenPrim.Add(prim); 1053 childrenPrim.Add(prim);
1052 1054
1053 foreach (OdePrim prm in childrenPrim) 1055 foreach (OdePrim prm in childrenPrim)
@@ -1080,7 +1082,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1080 m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet"); 1082 m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet");
1081 continue; 1083 continue;
1082 } 1084 }
1083//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + m_primName); 1085//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + m_primName);
1084 d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); 1086 d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
1085 d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); 1087 d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
1086 1088
@@ -1128,7 +1130,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1128 m_collisionCategories |= CollisionCategories.Body; 1130 m_collisionCategories |= CollisionCategories.Body;
1129 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); 1131 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1130 1132
1131//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + m_primName); 1133//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + m_primName);
1132 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 1134 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1133//Console.WriteLine(" Post GeomSetCategoryBits 2"); 1135//Console.WriteLine(" Post GeomSetCategoryBits 2");
1134 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 1136 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
@@ -1203,7 +1205,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1203 { 1205 {
1204 foreach (OdePrim prm in childrenPrim) 1206 foreach (OdePrim prm in childrenPrim)
1205 { 1207 {
1206//Console.WriteLine("ChildSetGeom calls ParentPrim"); 1208//Console.WriteLine("ChildSetGeom calls ParentPrim");
1207 ParentPrim(prm); 1209 ParentPrim(prm);
1208 } 1210 }
1209 } 1211 }
@@ -1230,7 +1232,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1230 1232
1231 lock (childrenPrim) 1233 lock (childrenPrim)
1232 { 1234 {
1233 //Console.WriteLine("childrenPrim.Remove " + odePrim); 1235 //Console.WriteLine("childrenPrim.Remove " + odePrim);
1234 childrenPrim.Remove(odePrim); 1236 childrenPrim.Remove(odePrim);
1235 } 1237 }
1236 1238
@@ -1248,7 +1250,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1248 { 1250 {
1249 foreach (OdePrim prm in childrenPrim) 1251 foreach (OdePrim prm in childrenPrim)
1250 { 1252 {
1251//Console.WriteLine("ChildDelink calls ParentPrim"); 1253//Console.WriteLine("ChildDelink calls ParentPrim");
1252 ParentPrim(prm); 1254 ParentPrim(prm);
1253 } 1255 }
1254 } 1256 }
@@ -1350,7 +1352,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1350 1352
1351 public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh) 1353 public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh)
1352 { 1354 {
1353//Console.WriteLine("CreateGeom:"); 1355//Console.WriteLine("CreateGeom:");
1354 if (_mesh != null) 1356 if (_mesh != null)
1355 { 1357 {
1356 setMesh(_parent_scene, _mesh); 1358 setMesh(_parent_scene, _mesh);
@@ -1381,7 +1383,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1381 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1383 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1382 try 1384 try
1383 { 1385 {
1384//Console.WriteLine(" CreateGeom 2"); 1386//Console.WriteLine(" CreateGeom 2");
1385 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); 1387 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1386 } 1388 }
1387 catch (AccessViolationException) 1389 catch (AccessViolationException)
@@ -1397,7 +1399,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1397 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1399 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1398 try 1400 try
1399 { 1401 {
1400//Console.WriteLine(" CreateGeom 3"); 1402//Console.WriteLine(" CreateGeom 3");
1401 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); 1403 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1402 } 1404 }
1403 catch (AccessViolationException) 1405 catch (AccessViolationException)
@@ -1414,7 +1416,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1414 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1416 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1415 try 1417 try
1416 { 1418 {
1417//Console.WriteLine(" CreateGeom 4"); 1419//Console.WriteLine(" CreateGeom 4");
1418 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); 1420 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1419 } 1421 }
1420 catch (AccessViolationException) 1422 catch (AccessViolationException)
@@ -1451,7 +1453,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1451 1453
1452 lock (_parent_scene.OdeLock) 1454 lock (_parent_scene.OdeLock)
1453 { 1455 {
1454//Console.WriteLine("changeadd 1"); 1456//Console.WriteLine("changeadd 1");
1455 CreateGeom(m_targetSpace, _mesh); 1457 CreateGeom(m_targetSpace, _mesh);
1456 1458
1457 if (prim_geom != IntPtr.Zero) 1459 if (prim_geom != IntPtr.Zero)
@@ -1508,7 +1510,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1508 if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body) 1510 if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body)
1509 { 1511 {
1510// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used?? 1512// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used??
1511Console.WriteLine(" JointCreateFixed"); 1513Console.WriteLine(" JointCreateFixed");
1512 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); 1514 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
1513 d.JointAttach(m_linkJoint, Body, odParent.Body); 1515 d.JointAttach(m_linkJoint, Body, odParent.Body);
1514 d.JointSetFixed(m_linkJoint); 1516 d.JointSetFixed(m_linkJoint);
@@ -1562,244 +1564,244 @@ Console.WriteLine(" JointCreateFixed");
1562 float fz = 0; 1564 float fz = 0;
1563 1565
1564 1566
1565 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims. 1567 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims.
1566 { 1568 {
1567 if (m_vehicle.Type != Vehicle.TYPE_NONE) 1569 if (m_vehicle.Type != Vehicle.TYPE_NONE)
1568 { 1570 {
1569 // 'VEHICLES' are dealt with in ODEDynamics.cs 1571 // 'VEHICLES' are dealt with in ODEDynamics.cs
1570 m_vehicle.Step(timestep, _parent_scene); 1572 m_vehicle.Step(timestep, _parent_scene);
1571 } 1573 }
1572 else 1574 else
1573 { 1575 {
1574//Console.WriteLine("Move " + m_primName); 1576//Console.WriteLine("Move " + m_primName);
1575 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 1577 if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
1576 // NON-'VEHICLES' are dealt with here 1578 // NON-'VEHICLES' are dealt with here
1577 if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f)) 1579 if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f))
1578 { 1580 {
1579 d.Vector3 avel2 = d.BodyGetAngularVel(Body); 1581 d.Vector3 avel2 = d.BodyGetAngularVel(Body);
1580 if (m_angularlock.X == 1) 1582 if (m_angularlock.X == 1)
1581 avel2.X = 0; 1583 avel2.X = 0;
1582 if (m_angularlock.Y == 1) 1584 if (m_angularlock.Y == 1)
1583 avel2.Y = 0; 1585 avel2.Y = 0;
1584 if (m_angularlock.Z == 1) 1586 if (m_angularlock.Z == 1)
1585 avel2.Z = 0; 1587 avel2.Z = 0;
1586 d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z); 1588 d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
1587 } 1589 }
1588 //float PID_P = 900.0f; 1590 //float PID_P = 900.0f;
1589 1591
1590 float m_mass = CalculateMass(); 1592 float m_mass = CalculateMass();
1591 1593
1592// fz = 0f; 1594// fz = 0f;
1593 //m_log.Info(m_collisionFlags.ToString()); 1595 //m_log.Info(m_collisionFlags.ToString());
1594 1596
1595 1597
1596 //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. 1598 //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
1597 // would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ?? 1599 // would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ??
1598 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up 1600 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
1599 // gravityz multiplier = 1 - m_buoyancy 1601 // gravityz multiplier = 1 - m_buoyancy
1600 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; 1602 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass;
1601 1603
1602 if (m_usePID) 1604 if (m_usePID)
1603 { 1605 {
1604//Console.WriteLine("PID " + m_primName); 1606//Console.WriteLine("PID " + m_primName);
1605 // KF - this is for object move? eg. llSetPos() ? 1607 // KF - this is for object move? eg. llSetPos() ?
1606 //if (!d.BodyIsEnabled(Body)) 1608 //if (!d.BodyIsEnabled(Body))
1607 //d.BodySetForce(Body, 0f, 0f, 0f); 1609 //d.BodySetForce(Body, 0f, 0f, 0f);
1608 // If we're using the PID controller, then we have no gravity 1610 // If we're using the PID controller, then we have no gravity
1609 //fz = (-1 * _parent_scene.gravityz) * m_mass; //KF: ?? Prims have no global gravity,so simply... 1611 //fz = (-1 * _parent_scene.gravityz) * m_mass; //KF: ?? Prims have no global gravity,so simply...
1610 fz = 0f; 1612 fz = 0f;
1611 1613
1612 // no lock; for now it's only called from within Simulate() 1614 // no lock; for now it's only called from within Simulate()
1613 1615
1614 // If the PID Controller isn't active then we set our force 1616 // If the PID Controller isn't active then we set our force
1615 // calculating base velocity to the current position 1617 // calculating base velocity to the current position
1616 1618
1617 if ((m_PIDTau < 1) && (m_PIDTau != 0)) 1619 if ((m_PIDTau < 1) && (m_PIDTau != 0))
1618 { 1620 {
1619 //PID_G = PID_G / m_PIDTau; 1621 //PID_G = PID_G / m_PIDTau;
1620 m_PIDTau = 1; 1622 m_PIDTau = 1;
1621 } 1623 }
1622 1624
1623 if ((PID_G - m_PIDTau) <= 0) 1625 if ((PID_G - m_PIDTau) <= 0)
1624 { 1626 {
1625 PID_G = m_PIDTau + 1; 1627 PID_G = m_PIDTau + 1;
1626 } 1628 }
1627 //PidStatus = true; 1629 //PidStatus = true;
1628 1630
1629 // PhysicsVector vec = new PhysicsVector(); 1631 // PhysicsVector vec = new PhysicsVector();
1630 d.Vector3 vel = d.BodyGetLinearVel(Body); 1632 d.Vector3 vel = d.BodyGetLinearVel(Body);
1631 1633
1632 d.Vector3 pos = d.BodyGetPosition(Body); 1634 d.Vector3 pos = d.BodyGetPosition(Body);
1633 _target_velocity = 1635 _target_velocity =
1634 new Vector3( 1636 new Vector3(
1635 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), 1637 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
1636 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), 1638 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
1637 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) 1639 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
1638 ); 1640 );
1639 1641
1640 // if velocity is zero, use position control; otherwise, velocity control 1642 // if velocity is zero, use position control; otherwise, velocity control
1641 1643
1642 if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f)) 1644 if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
1643 { 1645 {
1644 // keep track of where we stopped. No more slippin' & slidin' 1646 // keep track of where we stopped. No more slippin' & slidin'
1645 1647
1646 // We only want to deactivate the PID Controller if we think we want to have our surrogate 1648 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1647 // react to the physics scene by moving it's position. 1649 // react to the physics scene by moving it's position.
1648 // Avatar to Avatar collisions 1650 // Avatar to Avatar collisions
1649 // Prim to avatar collisions 1651 // Prim to avatar collisions
1650 1652
1651 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); 1653 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
1652 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); 1654 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
1653 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; 1655 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
1654 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); 1656 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
1655 d.BodySetLinearVel(Body, 0, 0, 0); 1657 d.BodySetLinearVel(Body, 0, 0, 0);
1656 d.BodyAddForce(Body, 0, 0, fz); 1658 d.BodyAddForce(Body, 0, 0, fz);
1657 return; 1659 return;
1658 } 1660 }
1659 else 1661 else
1660 { 1662 {
1661 _zeroFlag = false; 1663 _zeroFlag = false;
1662 1664
1663 // We're flying and colliding with something 1665 // We're flying and colliding with something
1664 fx = ((_target_velocity.X) - vel.X) * (PID_D); 1666 fx = ((_target_velocity.X) - vel.X) * (PID_D);
1665 fy = ((_target_velocity.Y) - vel.Y) * (PID_D); 1667 fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
1666 1668
1667 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; 1669 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
1668 1670
1669 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); 1671 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
1670 } 1672 }
1671 } // end if (m_usePID) 1673 } // end if (m_usePID)
1672 1674
1673 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller 1675 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
1674 if (m_useHoverPID && !m_usePID) 1676 if (m_useHoverPID && !m_usePID)
1675 { 1677 {
1676//Console.WriteLine("Hover " + m_primName); 1678//Console.WriteLine("Hover " + m_primName);
1677
1678 // If we're using the PID controller, then we have no gravity
1679 fz = (-1 * _parent_scene.gravityz) * m_mass;
1680
1681 // no lock; for now it's only called from within Simulate()
1682
1683 // If the PID Controller isn't active then we set our force
1684 // calculating base velocity to the current position
1685
1686 if ((m_PIDTau < 1))
1687 {
1688 PID_G = PID_G / m_PIDTau;
1689 }
1690
1691 if ((PID_G - m_PIDTau) <= 0)
1692 {
1693 PID_G = m_PIDTau + 1;
1694 }
1695 1679
1680 // If we're using the PID controller, then we have no gravity
1681 fz = (-1 * _parent_scene.gravityz) * m_mass;
1682
1683 // no lock; for now it's only called from within Simulate()
1696 1684
1697 // Where are we, and where are we headed? 1685 // If the PID Controller isn't active then we set our force
1698 d.Vector3 pos = d.BodyGetPosition(Body); 1686 // calculating base velocity to the current position
1699 d.Vector3 vel = d.BodyGetLinearVel(Body); 1687
1700 1688 if ((m_PIDTau < 1))
1701 1689 {
1702 // Non-Vehicles have a limited set of Hover options. 1690 PID_G = PID_G / m_PIDTau;
1703 // determine what our target height really is based on HoverType 1691 }
1704 switch (m_PIDHoverType) 1692
1705 { 1693 if ((PID_G - m_PIDTau) <= 0)
1706 case PIDHoverType.Ground: 1694 {
1707 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); 1695 PID_G = m_PIDTau + 1;
1708 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; 1696 }
1709 break; 1697
1710 case PIDHoverType.GroundAndWater: 1698
1711 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); 1699 // Where are we, and where are we headed?
1712 m_waterHeight = _parent_scene.GetWaterLevel(); 1700 d.Vector3 pos = d.BodyGetPosition(Body);
1713 if (m_groundHeight > m_waterHeight) 1701 d.Vector3 vel = d.BodyGetLinearVel(Body);
1714 { 1702
1715 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; 1703
1716 } 1704 // Non-Vehicles have a limited set of Hover options.
1717 else 1705 // determine what our target height really is based on HoverType
1718 { 1706 switch (m_PIDHoverType)
1719 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; 1707 {
1720 } 1708 case PIDHoverType.Ground:
1721 break; 1709 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
1722 1710 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
1723 } // end switch (m_PIDHoverType) 1711 break;
1724 1712 case PIDHoverType.GroundAndWater:
1725 1713 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
1726 _target_velocity = 1714 m_waterHeight = _parent_scene.GetWaterLevel();
1715 if (m_groundHeight > m_waterHeight)
1716 {
1717 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
1718 }
1719 else
1720 {
1721 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
1722 }
1723 break;
1724
1725 } // end switch (m_PIDHoverType)
1726
1727
1728 _target_velocity =
1727 new Vector3(0.0f, 0.0f, 1729 new Vector3(0.0f, 0.0f,
1728 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) 1730 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
1729 ); 1731 );
1730 1732
1731 // if velocity is zero, use position control; otherwise, velocity control 1733 // if velocity is zero, use position control; otherwise, velocity control
1732 1734
1733 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) 1735 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
1734 { 1736 {
1735 // keep track of where we stopped. No more slippin' & slidin' 1737 // keep track of where we stopped. No more slippin' & slidin'
1736 1738
1737 // We only want to deactivate the PID Controller if we think we want to have our surrogate 1739 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1738 // react to the physics scene by moving it's position. 1740 // react to the physics scene by moving it's position.
1739 // Avatar to Avatar collisions 1741 // Avatar to Avatar collisions
1740 // Prim to avatar collisions 1742 // Prim to avatar collisions
1741 1743
1742 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); 1744 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
1743 d.BodySetLinearVel(Body, vel.X, vel.Y, 0); 1745 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
1744 d.BodyAddForce(Body, 0, 0, fz); 1746 d.BodyAddForce(Body, 0, 0, fz);
1745 return; 1747 return;
1746 } 1748 }
1747 else 1749 else
1748 { 1750 {
1749 _zeroFlag = false; 1751 _zeroFlag = false;
1750 1752
1751 // We're flying and colliding with something 1753 // We're flying and colliding with something
1752 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); 1754 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
1753 } 1755 }
1754 } 1756 }
1755 1757
1756 fx *= m_mass; 1758 fx *= m_mass;
1757 fy *= m_mass; 1759 fy *= m_mass;
1758 //fz *= m_mass; 1760 //fz *= m_mass;
1759 1761
1760 fx += m_force.X; 1762 fx += m_force.X;
1761 fy += m_force.Y; 1763 fy += m_force.Y;
1762 fz += m_force.Z; 1764 fz += m_force.Z;
1763 1765
1764 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); 1766 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
1765 if (fx != 0 || fy != 0 || fz != 0) 1767 if (fx != 0 || fy != 0 || fz != 0)
1766 { 1768 {
1767 //m_taintdisable = true; 1769 //m_taintdisable = true;
1768 //base.RaiseOutOfBounds(Position); 1770 //base.RaiseOutOfBounds(Position);
1769 //d.BodySetLinearVel(Body, fx, fy, 0f); 1771 //d.BodySetLinearVel(Body, fx, fy, 0f);
1770 if (!d.BodyIsEnabled(Body)) 1772 if (!d.BodyIsEnabled(Body))
1771 { 1773 {
1772 // A physical body at rest on a surface will auto-disable after a while, 1774 // A physical body at rest on a surface will auto-disable after a while,
1773 // this appears to re-enable it incase the surface it is upon vanishes, 1775 // this appears to re-enable it incase the surface it is upon vanishes,
1774 // and the body should fall again. 1776 // and the body should fall again.
1775 d.BodySetLinearVel(Body, 0f, 0f, 0f); 1777 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1776 d.BodySetForce(Body, 0, 0, 0); 1778 d.BodySetForce(Body, 0, 0, 0);
1777 enableBodySoft(); 1779 enableBodySoft();
1778 } 1780 }
1779 1781
1780 // 35x10 = 350n times the mass per second applied maximum. 1782 // 35x10 = 350n times the mass per second applied maximum.
1781 float nmax = 35f * m_mass; 1783 float nmax = 35f * m_mass;
1782 float nmin = -35f * m_mass; 1784 float nmin = -35f * m_mass;
1783 1785
1784 1786
1785 if (fx > nmax) 1787 if (fx > nmax)
1786 fx = nmax; 1788 fx = nmax;
1787 if (fx < nmin) 1789 if (fx < nmin)
1788 fx = nmin; 1790 fx = nmin;
1789 if (fy > nmax) 1791 if (fy > nmax)
1790 fy = nmax; 1792 fy = nmax;
1791 if (fy < nmin) 1793 if (fy < nmin)
1792 fy = nmin; 1794 fy = nmin;
1793 d.BodyAddForce(Body, fx, fy, fz); 1795 d.BodyAddForce(Body, fx, fy, fz);
1794//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); 1796//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
1795 } 1797 }
1796 } 1798 }
1797 } 1799 }
1798 else 1800 else
1799 { // is not physical, or is not a body or is selected 1801 { // is not physical, or is not a body or is selected
1800 // _zeroPosition = d.BodyGetPosition(Body); 1802 // _zeroPosition = d.BodyGetPosition(Body);
1801 return; 1803 return;
1802//Console.WriteLine("Nothing " + m_primName); 1804//Console.WriteLine("Nothing " + m_primName);
1803 1805
1804 } 1806 }
1805 } 1807 }
@@ -1815,18 +1817,18 @@ Console.WriteLine(" JointCreateFixed");
1815 myrot.W = _orientation.W; 1817 myrot.W = _orientation.W;
1816 if (Body != IntPtr.Zero) 1818 if (Body != IntPtr.Zero)
1817 { 1819 {
1818 // KF: If this is a root prim do BodySet 1820 // KF: If this is a root prim do BodySet
1819 d.BodySetQuaternion(Body, ref myrot); 1821 d.BodySetQuaternion(Body, ref myrot);
1820 if (m_isphysical) 1822 if (m_isphysical)
1821 { 1823 {
1822 if (!m_angularlock.ApproxEquals(Vector3.One, 0f)) 1824 if (!m_angularlock.ApproxEquals(Vector3.One, 0f))
1823 createAMotor(m_angularlock); 1825 createAMotor(m_angularlock);
1824 } 1826 }
1825 } 1827 }
1826 else 1828 else
1827 { 1829 {
1828 // daughter prim, do Geom set 1830 // daughter prim, do Geom set
1829 d.GeomSetQuaternion(prim_geom, ref myrot); 1831 d.GeomSetQuaternion(prim_geom, ref myrot);
1830 } 1832 }
1831 1833
1832 resetCollisionAccounting(); 1834 resetCollisionAccounting();
@@ -1890,7 +1892,7 @@ Console.WriteLine(" JointCreateFixed");
1890 m_log.Error("[PHYSICS]: PrimGeom dead"); 1892 m_log.Error("[PHYSICS]: PrimGeom dead");
1891 } 1893 }
1892 } 1894 }
1893//Console.WriteLine("changePhysicsStatus for " + m_primName ); 1895//Console.WriteLine("changePhysicsStatus for " + m_primName);
1894 changeadd(2f); 1896 changeadd(2f);
1895 } 1897 }
1896 if (childPrim) 1898 if (childPrim)
@@ -1976,7 +1978,7 @@ Console.WriteLine(" JointCreateFixed");
1976 else 1978 else
1977 { 1979 {
1978 _mesh = null; 1980 _mesh = null;
1979//Console.WriteLine("changesize 2"); 1981//Console.WriteLine("changesize 2");
1980 CreateGeom(m_targetSpace, _mesh); 1982 CreateGeom(m_targetSpace, _mesh);
1981 } 1983 }
1982 1984
@@ -2083,7 +2085,7 @@ Console.WriteLine(" JointCreateFixed");
2083 else 2085 else
2084 { 2086 {
2085 _mesh = null; 2087 _mesh = null;
2086//Console.WriteLine("changeshape"); 2088//Console.WriteLine("changeshape");
2087 CreateGeom(m_targetSpace, null); 2089 CreateGeom(m_targetSpace, null);
2088 } 2090 }
2089 2091
@@ -2454,7 +2456,7 @@ Console.WriteLine(" JointCreateFixed");
2454 if (QuaternionIsFinite(value)) 2456 if (QuaternionIsFinite(value))
2455 { 2457 {
2456 _orientation = value; 2458 _orientation = value;
2457 } 2459 }
2458 else 2460 else
2459 m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object"); 2461 m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object");
2460 2462
@@ -2675,8 +2677,8 @@ Console.WriteLine(" JointCreateFixed");
2675 //outofBounds = true; 2677 //outofBounds = true;
2676 } 2678 }
2677 2679
2678 //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)); 2680 //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation));
2679//Console.WriteLine("Adiff " + m_primName + " = " + Adiff); 2681//Console.WriteLine("Adiff " + m_primName + " = " + Adiff);
2680 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) 2682 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
2681 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) 2683 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
2682 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) 2684 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
@@ -2684,7 +2686,7 @@ Console.WriteLine(" JointCreateFixed");
2684 && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large 2686 && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large
2685 { 2687 {
2686 _zeroFlag = true; 2688 _zeroFlag = true;
2687//Console.WriteLine("ZFT 2"); 2689//Console.WriteLine("ZFT 2");
2688 m_throttleUpdates = false; 2690 m_throttleUpdates = false;
2689 } 2691 }
2690 else 2692 else
generated by cgit v1.1 at 2024-11-18 16:55:13 +0000