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.cs589
1 files changed, 323 insertions, 266 deletions
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index 4581d22..864ea80 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -1,4 +1,15 @@
1/* 1/*
2 * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces
3 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
4 * ODEPrim.cs contains methods dealing with Prim editing, Prim
5 * characteristics and Kinetic motion.
6 * ODEDynamics.cs contains methods dealing with Prim Physical motion
7 * (dynamics) and the associated settings. Old Linear and angular
8 * motors for dynamic motion have been replace with MoveLinear()
9 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
10 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
11 * switch between 'VEHICLE' parameter use and general dynamics
12 * settings use.
2 * Copyright (c) Contributors, http://opensimulator.org/ 13 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders. 14 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 * 15 *
@@ -72,6 +83,9 @@ namespace OpenSim.Region.Physics.OdePlugin
72 private float PID_G = 25f; 83 private float PID_G = 25f;
73 private bool m_usePID = false; 84 private bool m_usePID = false;
74 85
86 // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
87 // and are for non-VEHICLES only.
88
75 private float m_PIDHoverHeight = 0f; 89 private float m_PIDHoverHeight = 0f;
76 private float m_PIDHoverTau = 0f; 90 private float m_PIDHoverTau = 0f;
77 private bool m_useHoverPID = false; 91 private bool m_useHoverPID = false;
@@ -79,6 +93,7 @@ namespace OpenSim.Region.Physics.OdePlugin
79 private float m_targetHoverHeight = 0f; 93 private float m_targetHoverHeight = 0f;
80 private float m_groundHeight = 0f; 94 private float m_groundHeight = 0f;
81 private float m_waterHeight = 0f; 95 private float m_waterHeight = 0f;
96 private float m_buoyancy = 0f; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
82 97
83 // private float m_tensor = 5f; 98 // private float m_tensor = 5f;
84 private int body_autodisable_frames = 20; 99 private int body_autodisable_frames = 20;
@@ -146,8 +161,6 @@ namespace OpenSim.Region.Physics.OdePlugin
146 public int m_roundsUnderMotionThreshold = 0; 161 public int m_roundsUnderMotionThreshold = 0;
147 private int m_crossingfailures = 0; 162 private int m_crossingfailures = 0;
148 163
149 public float m_buoyancy = 0f;
150
151 public bool outofBounds = false; 164 public bool outofBounds = false;
152 private float m_density = 10.000006836f; // Aluminum g/cm3; 165 private float m_density = 10.000006836f; // Aluminum g/cm3;
153 166
@@ -155,7 +168,8 @@ namespace OpenSim.Region.Physics.OdePlugin
155 private bool m_lastUpdateSent = false; 168 private bool m_lastUpdateSent = false;
156 169
157 public IntPtr Body = (IntPtr) 0; 170 public IntPtr Body = (IntPtr) 0;
158 private String m_primName; 171 public String m_primName;
172// private String m_primName;
159 private PhysicsVector _target_velocity; 173 private PhysicsVector _target_velocity;
160 public d.Mass pMass; 174 public d.Mass pMass;
161 175
@@ -166,7 +180,7 @@ namespace OpenSim.Region.Physics.OdePlugin
166 180
167 public volatile bool childPrim = false; 181 public volatile bool childPrim = false;
168 182
169 private ODEVehicleSettings m_vehicle; 183 private ODEDynamics m_vehicle;
170 184
171 internal int m_material = (int)Material.Wood; 185 internal int m_material = (int)Material.Wood;
172 186
@@ -174,7 +188,7 @@ namespace OpenSim.Region.Physics.OdePlugin
174 Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode) 188 Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode)
175 { 189 {
176 _target_velocity = new PhysicsVector(0, 0, 0); 190 _target_velocity = new PhysicsVector(0, 0, 0);
177 m_vehicle = new ODEVehicleSettings(); 191 m_vehicle = new ODEDynamics();
178 //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned); 192 //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
179 ode = dode; 193 ode = dode;
180 _velocity = new PhysicsVector(); 194 _velocity = new PhysicsVector();
@@ -267,6 +281,8 @@ namespace OpenSim.Region.Physics.OdePlugin
267 public override bool Selected 281 public override bool Selected
268 { 282 {
269 set { 283 set {
284
285
270 // This only makes the object not collidable if the object 286 // This only makes the object not collidable if the object
271 // is physical or the object is modified somehow *IN THE FUTURE* 287 // is physical or the object is modified somehow *IN THE FUTURE*
272 // without this, if an avatar selects prim, they can walk right 288 // without this, if an avatar selects prim, they can walk right
@@ -282,6 +298,7 @@ namespace OpenSim.Region.Physics.OdePlugin
282 m_taintselected = value; 298 m_taintselected = value;
283 m_isSelected = value; 299 m_isSelected = value;
284 } 300 }
301 if(m_isSelected) disableBodySoft();
285 } 302 }
286 } 303 }
287 304
@@ -289,6 +306,7 @@ namespace OpenSim.Region.Physics.OdePlugin
289 { 306 {
290 prev_geom = prim_geom; 307 prev_geom = prim_geom;
291 prim_geom = geom; 308 prim_geom = geom;
309//Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName);
292 if (prim_geom != IntPtr.Zero) 310 if (prim_geom != IntPtr.Zero)
293 { 311 {
294 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 312 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
@@ -300,6 +318,7 @@ namespace OpenSim.Region.Physics.OdePlugin
300 if (_parent != null && _parent is OdePrim) 318 if (_parent != null && _parent is OdePrim)
301 { 319 {
302 OdePrim parent = (OdePrim)_parent; 320 OdePrim parent = (OdePrim)_parent;
321//Console.WriteLine("SetGeom calls ChildSetGeom");
303 parent.ChildSetGeom(this); 322 parent.ChildSetGeom(this);
304 } 323 }
305 } 324 }
@@ -315,7 +334,8 @@ namespace OpenSim.Region.Physics.OdePlugin
315 if (m_isphysical && Body != IntPtr.Zero) 334 if (m_isphysical && Body != IntPtr.Zero)
316 { 335 {
317 d.BodyEnable(Body); 336 d.BodyEnable(Body);
318 m_vehicle.Enable(Body, _parent_scene); 337 if (m_vehicle.Type != Vehicle.TYPE_NONE)
338 m_vehicle.Enable(Body, _parent_scene);
319 } 339 }
320 340
321 m_disabled = false; 341 m_disabled = false;
@@ -329,7 +349,6 @@ namespace OpenSim.Region.Physics.OdePlugin
329 if (m_isphysical && Body != IntPtr.Zero) 349 if (m_isphysical && Body != IntPtr.Zero)
330 { 350 {
331 d.BodyDisable(Body); 351 d.BodyDisable(Body);
332 m_vehicle.Disable();
333 } 352 }
334 } 353 }
335 354
@@ -359,6 +378,9 @@ namespace OpenSim.Region.Physics.OdePlugin
359 378
360 d.BodySetAutoDisableFlag(Body, true); 379 d.BodySetAutoDisableFlag(Body, true);
361 d.BodySetAutoDisableSteps(Body, body_autodisable_frames); 380 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
381
382 // disconnect from world gravity so we can apply buoyancy
383 d.BodySetGravityMode (Body, false);
362 384
363 m_interpenetrationcount = 0; 385 m_interpenetrationcount = 0;
364 m_collisionscore = 0; 386 m_collisionscore = 0;
@@ -705,13 +727,8 @@ namespace OpenSim.Region.Physics.OdePlugin
705 break; 727 break;
706 } 728 }
707 } 729 }
708
709
710
711
712
713 return returnMass; 730 return returnMass;
714 } 731 }// end CalculateMass
715 732
716 #endregion 733 #endregion
717 734
@@ -737,7 +754,6 @@ namespace OpenSim.Region.Physics.OdePlugin
737 if (Body != IntPtr.Zero) 754 if (Body != IntPtr.Zero)
738 { 755 {
739 _parent_scene.remActivePrim(this); 756 _parent_scene.remActivePrim(this);
740 m_vehicle.Destroy();
741 m_collisionCategories &= ~CollisionCategories.Body; 757 m_collisionCategories &= ~CollisionCategories.Body;
742 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); 758 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
743 759
@@ -827,6 +843,7 @@ namespace OpenSim.Region.Physics.OdePlugin
827 { 843 {
828 if (prim_geom == IntPtr.Zero) 844 if (prim_geom == IntPtr.Zero)
829 { 845 {
846//Console.WriteLine(" setMesh 1");
830 SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null)); 847 SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null));
831 } 848 }
832 } 849 }
@@ -848,19 +865,35 @@ namespace OpenSim.Region.Physics.OdePlugin
848 865
849 public void ProcessTaints(float timestep) 866 public void ProcessTaints(float timestep)
850 { 867 {
868//Console.WriteLine("ProcessTaints for " + m_primName );
851 if (m_taintadd) 869 if (m_taintadd)
852 { 870 {
853 changeadd(timestep); 871 changeadd(timestep);
854 } 872 }
873
855 if (prim_geom != IntPtr.Zero) 874 if (prim_geom != IntPtr.Zero)
856 { 875 {
857 if (!_position.IsIdentical(m_taintposition,0f)) 876 if (!_position.IsIdentical(m_taintposition,0f))
858 changemove(timestep); 877 changemove(timestep);
859 878
860 if (m_taintrot != _orientation) 879 if (m_taintrot != _orientation)
861 rotate(timestep); 880 {
881 if(childPrim && IsPhysical) // For physical child prim...
882 {
883 rotate(timestep);
884 // KF: ODE will also rotate the parent prim!
885 // so rotate the root back to where it was
886 OdePrim parent = (OdePrim)_parent;
887 parent.rotate(timestep);
888 }
889 else
890 {
891 //Just rotate the prim
892 rotate(timestep);
893 }
894 }
862 // 895 //
863 896
864 if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent)) 897 if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent))
865 changePhysicsStatus(timestep); 898 changePhysicsStatus(timestep);
866 // 899 //
@@ -899,8 +932,7 @@ namespace OpenSim.Region.Physics.OdePlugin
899 932
900 if (!m_angularlock.IsIdentical(m_taintAngularLock,0)) 933 if (!m_angularlock.IsIdentical(m_taintAngularLock,0))
901 changeAngularLock(timestep); 934 changeAngularLock(timestep);
902 935
903
904 } 936 }
905 else 937 else
906 { 938 {
@@ -932,11 +964,6 @@ namespace OpenSim.Region.Physics.OdePlugin
932 Amotor = IntPtr.Zero; 964 Amotor = IntPtr.Zero;
933 } 965 }
934 } 966 }
935
936 if (m_vehicle.Type != Vehicle.TYPE_NONE)
937 {
938 m_vehicle.Reset();
939 }
940 } 967 }
941 } 968 }
942 // Store this for later in case we get turned into a separate body 969 // Store this for later in case we get turned into a separate body
@@ -954,7 +981,7 @@ namespace OpenSim.Region.Physics.OdePlugin
954 { 981 {
955 OdePrim obj = (OdePrim)m_taintparent; 982 OdePrim obj = (OdePrim)m_taintparent;
956 //obj.disableBody(); 983 //obj.disableBody();
957 984//Console.WriteLine("changelink calls ParentPrim");
958 obj.ParentPrim(this); 985 obj.ParentPrim(this);
959 986
960 /* 987 /*
@@ -972,6 +999,8 @@ namespace OpenSim.Region.Physics.OdePlugin
972 // destroy link 999 // destroy link
973 else if (_parent != null && m_taintparent == null) 1000 else if (_parent != null && m_taintparent == null)
974 { 1001 {
1002//Console.WriteLine(" changelink B");
1003
975 if (_parent is OdePrim) 1004 if (_parent is OdePrim)
976 { 1005 {
977 OdePrim obj = (OdePrim)_parent; 1006 OdePrim obj = (OdePrim)_parent;
@@ -988,7 +1017,7 @@ namespace OpenSim.Region.Physics.OdePlugin
988 m_linkJoint = (IntPtr)0; 1017 m_linkJoint = (IntPtr)0;
989 */ 1018 */
990 } 1019 }
991 1020
992 _parent = m_taintparent; 1021 _parent = m_taintparent;
993 m_taintPhysics = m_isphysical; 1022 m_taintPhysics = m_isphysical;
994 } 1023 }
@@ -997,6 +1026,7 @@ namespace OpenSim.Region.Physics.OdePlugin
997 // prim is the child 1026 // prim is the child
998 public void ParentPrim(OdePrim prim) 1027 public void ParentPrim(OdePrim prim)
999 { 1028 {
1029//Console.WriteLine("ParentPrim " + m_primName);
1000 if (this.m_localID != prim.m_localID) 1030 if (this.m_localID != prim.m_localID)
1001 { 1031 {
1002 if (Body == IntPtr.Zero) 1032 if (Body == IntPtr.Zero)
@@ -1010,6 +1040,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1010 { 1040 {
1011 if (!childrenPrim.Contains(prim)) 1041 if (!childrenPrim.Contains(prim))
1012 { 1042 {
1043//Console.WriteLine("childrenPrim.Add " + prim);
1013 childrenPrim.Add(prim); 1044 childrenPrim.Add(prim);
1014 1045
1015 foreach (OdePrim prm in childrenPrim) 1046 foreach (OdePrim prm in childrenPrim)
@@ -1033,6 +1064,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1033 } 1064 }
1034 foreach (OdePrim prm in childrenPrim) 1065 foreach (OdePrim prm in childrenPrim)
1035 { 1066 {
1067
1036 prm.m_collisionCategories |= CollisionCategories.Body; 1068 prm.m_collisionCategories |= CollisionCategories.Body;
1037 prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); 1069 prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1038 1070
@@ -1041,7 +1073,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1041 m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet"); 1073 m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet");
1042 continue; 1074 continue;
1043 } 1075 }
1044 1076//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + m_primName);
1045 d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); 1077 d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
1046 d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); 1078 d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
1047 1079
@@ -1086,11 +1118,12 @@ namespace OpenSim.Region.Physics.OdePlugin
1086 prm.Body = Body; 1118 prm.Body = Body;
1087 _parent_scene.addActivePrim(prm); 1119 _parent_scene.addActivePrim(prm);
1088 } 1120 }
1089
1090 m_collisionCategories |= CollisionCategories.Body; 1121 m_collisionCategories |= CollisionCategories.Body;
1091 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); 1122 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1092 1123
1124//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + m_primName);
1093 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 1125 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1126//Console.WriteLine(" Post GeomSetCategoryBits 2");
1094 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 1127 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1095 1128
1096 1129
@@ -1126,7 +1159,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1126 createAMotor(m_angularlock); 1159 createAMotor(m_angularlock);
1127 } 1160 }
1128 d.BodySetPosition(Body, Position.X, Position.Y, Position.Z); 1161 d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);
1129 m_vehicle.Enable(Body, _parent_scene); 1162 if (m_vehicle.Type != Vehicle.TYPE_NONE) m_vehicle.Enable(Body, _parent_scene);
1130 _parent_scene.addActivePrim(this); 1163 _parent_scene.addActivePrim(this);
1131 } 1164 }
1132 } 1165 }
@@ -1163,6 +1196,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1163 { 1196 {
1164 foreach (OdePrim prm in childrenPrim) 1197 foreach (OdePrim prm in childrenPrim)
1165 { 1198 {
1199//Console.WriteLine("ChildSetGeom calls ParentPrim");
1166 ParentPrim(prm); 1200 ParentPrim(prm);
1167 } 1201 }
1168 } 1202 }
@@ -1189,6 +1223,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1189 1223
1190 lock (childrenPrim) 1224 lock (childrenPrim)
1191 { 1225 {
1226 //Console.WriteLine("childrenPrim.Remove " + odePrim);
1192 childrenPrim.Remove(odePrim); 1227 childrenPrim.Remove(odePrim);
1193 } 1228 }
1194 1229
@@ -1206,6 +1241,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1206 { 1241 {
1207 foreach (OdePrim prm in childrenPrim) 1242 foreach (OdePrim prm in childrenPrim)
1208 { 1243 {
1244//Console.WriteLine("ChildDelink calls ParentPrim");
1209 ParentPrim(prm); 1245 ParentPrim(prm);
1210 } 1246 }
1211 } 1247 }
@@ -1290,7 +1326,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1290 1326
1291 resetCollisionAccounting(); 1327 resetCollisionAccounting();
1292 m_isSelected = m_taintselected; 1328 m_isSelected = m_taintselected;
1293 } 1329 }//end changeSelectedStatus
1294 1330
1295 public void ResetTaints() 1331 public void ResetTaints()
1296 { 1332 {
@@ -1307,6 +1343,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1307 1343
1308 public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh) 1344 public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh)
1309 { 1345 {
1346//Console.WriteLine("CreateGeom:");
1310 if (_mesh != null) 1347 if (_mesh != null)
1311 { 1348 {
1312 setMesh(_parent_scene, _mesh); 1349 setMesh(_parent_scene, _mesh);
@@ -1322,6 +1359,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1322 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1359 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1323 try 1360 try
1324 { 1361 {
1362//Console.WriteLine(" CreateGeom 1");
1325 SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2)); 1363 SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2));
1326 } 1364 }
1327 catch (AccessViolationException) 1365 catch (AccessViolationException)
@@ -1336,6 +1374,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1336 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1374 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1337 try 1375 try
1338 { 1376 {
1377//Console.WriteLine(" CreateGeom 2");
1339 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); 1378 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1340 } 1379 }
1341 catch (AccessViolationException) 1380 catch (AccessViolationException)
@@ -1351,6 +1390,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1351 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1390 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1352 try 1391 try
1353 { 1392 {
1393//Console.WriteLine(" CreateGeom 3");
1354 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); 1394 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1355 } 1395 }
1356 catch (AccessViolationException) 1396 catch (AccessViolationException)
@@ -1367,6 +1407,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1367 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1407 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1368 try 1408 try
1369 { 1409 {
1410//Console.WriteLine(" CreateGeom 4");
1370 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); 1411 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1371 } 1412 }
1372 catch (AccessViolationException) 1413 catch (AccessViolationException)
@@ -1403,6 +1444,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1403 1444
1404 lock (_parent_scene.OdeLock) 1445 lock (_parent_scene.OdeLock)
1405 { 1446 {
1447//Console.WriteLine("changeadd 1");
1406 CreateGeom(m_targetSpace, _mesh); 1448 CreateGeom(m_targetSpace, _mesh);
1407 1449
1408 if (prim_geom != IntPtr.Zero) 1450 if (prim_geom != IntPtr.Zero)
@@ -1458,6 +1500,8 @@ namespace OpenSim.Region.Physics.OdePlugin
1458 OdePrim odParent = (OdePrim)_parent; 1500 OdePrim odParent = (OdePrim)_parent;
1459 if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body) 1501 if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body)
1460 { 1502 {
1503// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used??
1504Console.WriteLine(" JointCreateFixed");
1461 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); 1505 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
1462 d.JointAttach(m_linkJoint, Body, odParent.Body); 1506 d.JointAttach(m_linkJoint, Body, odParent.Body);
1463 d.JointSetFixed(m_linkJoint); 1507 d.JointSetFixed(m_linkJoint);
@@ -1511,239 +1555,236 @@ namespace OpenSim.Region.Physics.OdePlugin
1511 float fz = 0; 1555 float fz = 0;
1512 1556
1513 1557
1514 if (IsPhysical && Body != IntPtr.Zero && !m_isSelected) 1558 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims.
1515 { 1559 {
1516 if (d.BodyIsEnabled(Body) && !m_angularlock.IsIdentical(PhysicsVector.Zero, 0.003f)) 1560 if (m_vehicle.Type != Vehicle.TYPE_NONE)
1517 { 1561 {
1518 d.Vector3 avel2 = d.BodyGetAngularVel(Body); 1562 // 'VEHICLES' are dealt with in ODEDynamics.cs
1519 if (m_angularlock.X == 1) 1563 m_vehicle.Step(timestep, _parent_scene);
1520 avel2.X = 0; 1564 }
1521 if (m_angularlock.Y == 1) 1565 else
1522 avel2.Y = 0; 1566 {
1523 if (m_angularlock.Z == 1) 1567 // NON-'VEHICLES' are dealt with here
1524 avel2.Z = 0; 1568 if (d.BodyIsEnabled(Body) && !m_angularlock.IsIdentical(PhysicsVector.Zero, 0.003f))
1525 d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z); 1569 {
1526 } 1570 d.Vector3 avel2 = d.BodyGetAngularVel(Body);
1527 //float PID_P = 900.0f; 1571 if (m_angularlock.X == 1)
1528 1572 avel2.X = 0;
1529 float m_mass = CalculateMass(); 1573 if (m_angularlock.Y == 1)
1530 1574 avel2.Y = 0;
1531 fz = 0f; 1575 if (m_angularlock.Z == 1)
1576 avel2.Z = 0;
1577 d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
1578 }
1579 //float PID_P = 900.0f;
1580
1581 float m_mass = CalculateMass();
1582
1583// fz = 0f;
1532 //m_log.Info(m_collisionFlags.ToString()); 1584 //m_log.Info(m_collisionFlags.ToString());
1533 1585
1534 if (m_buoyancy != 0) 1586
1535 { 1587 //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
1536 if (m_buoyancy > 0) 1588 // would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ??
1537 { 1589 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
1538 fz = (((-1 * _parent_scene.gravityz) * m_buoyancy) * m_mass); 1590 // gravityz multiplier = 1 - m_buoyancy
1539 1591 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass;
1540 //d.Vector3 l_velocity = d.BodyGetLinearVel(Body); 1592
1541 //m_log.Info("Using Buoyancy: " + buoyancy + " G: " + (_parent_scene.gravityz * m_buoyancy) + "mass:" + m_mass + " Pos: " + Position.ToString()); 1593 if (m_usePID)
1542 } 1594 {
1543 else 1595 // KF - this is for object move? eg. llSetPos() ?
1544 { 1596 //if (!d.BodyIsEnabled(Body))
1545 fz = (-1 * (((-1 * _parent_scene.gravityz) * (-1 * m_buoyancy)) * m_mass)); 1597 //d.BodySetForce(Body, 0f, 0f, 0f);
1546 } 1598 // If we're using the PID controller, then we have no gravity
1547 } 1599 //fz = (-1 * _parent_scene.gravityz) * m_mass; //KF: ?? Prims have no global gravity,so simply...
1548 1600 fz = 0f;
1549 if (m_usePID) 1601
1550 { 1602 // no lock; for now it's only called from within Simulate()
1551 1603
1552 //if (!d.BodyIsEnabled(Body)) 1604 // If the PID Controller isn't active then we set our force
1553 //d.BodySetForce(Body, 0f, 0f, 0f); 1605 // calculating base velocity to the current position
1554 // If we're using the PID controller, then we have no gravity 1606
1555 fz = (-1 * _parent_scene.gravityz) * m_mass; 1607 if ((m_PIDTau < 1) && (m_PIDTau != 0))
1556 1608 {
1557 // no lock; for now it's only called from within Simulate() 1609 //PID_G = PID_G / m_PIDTau;
1558 1610 m_PIDTau = 1;
1559 // If the PID Controller isn't active then we set our force 1611 }
1560 // calculating base velocity to the current position 1612
1561 1613 if ((PID_G - m_PIDTau) <= 0)
1562 if ((m_PIDTau < 1) && (m_PIDTau != 0)) 1614 {
1563 { 1615 PID_G = m_PIDTau + 1;
1564 //PID_G = PID_G / m_PIDTau; 1616 }
1565 m_PIDTau = 1; 1617 //PidStatus = true;
1566 } 1618
1567 1619 // PhysicsVector vec = new PhysicsVector();
1568 if ((PID_G - m_PIDTau) <= 0) 1620 d.Vector3 vel = d.BodyGetLinearVel(Body);
1569 { 1621
1570 PID_G = m_PIDTau + 1; 1622 d.Vector3 pos = d.BodyGetPosition(Body);
1571 } 1623 _target_velocity =
1572 //PidStatus = true; 1624 new PhysicsVector(
1573 1625 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
1574 // PhysicsVector vec = new PhysicsVector(); 1626 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
1575 d.Vector3 vel = d.BodyGetLinearVel(Body); 1627 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
1576 1628 );
1577 d.Vector3 pos = d.BodyGetPosition(Body); 1629
1578 _target_velocity = 1630 // if velocity is zero, use position control; otherwise, velocity control
1579 new PhysicsVector( 1631
1580 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), 1632 if (_target_velocity.IsIdentical(PhysicsVector.Zero,0.1f))
1581 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), 1633 {
1582 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) 1634 // keep track of where we stopped. No more slippin' & slidin'
1583 ); 1635
1584 1636 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1585 // if velocity is zero, use position control; otherwise, velocity control 1637 // react to the physics scene by moving it's position.
1586 1638 // Avatar to Avatar collisions
1587 if (_target_velocity.IsIdentical(PhysicsVector.Zero,0.1f)) 1639 // Prim to avatar collisions
1588 { 1640
1589 // keep track of where we stopped. No more slippin' & slidin' 1641 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
1590 1642 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
1591 // We only want to deactivate the PID Controller if we think we want to have our surrogate 1643 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
1592 // react to the physics scene by moving it's position. 1644 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
1593 // Avatar to Avatar collisions 1645 d.BodySetLinearVel(Body, 0, 0, 0);
1594 // Prim to avatar collisions 1646 d.BodyAddForce(Body, 0, 0, fz);
1595 1647 return;
1596 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); 1648 }
1597 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); 1649 else
1598 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; 1650 {
1599 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); 1651 _zeroFlag = false;
1600 d.BodySetLinearVel(Body, 0, 0, 0); 1652
1601 d.BodyAddForce(Body, 0, 0, fz); 1653 // We're flying and colliding with something
1602 return; 1654 fx = ((_target_velocity.X) - vel.X) * (PID_D);
1603 } 1655 fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
1604 else 1656
1605 { 1657 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
1606 _zeroFlag = false; 1658
1607 1659 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
1608 // We're flying and colliding with something 1660 }
1609 fx = ((_target_velocity.X) - vel.X) * (PID_D); 1661 } // end if (m_usePID)
1610 fy = ((_target_velocity.Y) - vel.Y) * (PID_D); 1662
1611 1663 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
1612 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; 1664 if (m_useHoverPID && !m_usePID)
1613 1665 {
1614 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); 1666 // If we're using the PID controller, then we have no gravity
1615 } 1667 fz = (-1 * _parent_scene.gravityz) * m_mass;
1616 } 1668
1617 1669 // no lock; for now it's only called from within Simulate()
1618 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller 1670
1619 if (m_useHoverPID && !m_usePID) 1671 // If the PID Controller isn't active then we set our force
1620 { 1672 // calculating base velocity to the current position
1621 // If we're using the PID controller, then we have no gravity 1673
1622 fz = (-1 * _parent_scene.gravityz) * m_mass; 1674 if ((m_PIDTau < 1))
1623 1675 {
1624 // no lock; for now it's only called from within Simulate() 1676 PID_G = PID_G / m_PIDTau;
1625 1677 }
1626 // If the PID Controller isn't active then we set our force 1678
1627 // calculating base velocity to the current position 1679 if ((PID_G - m_PIDTau) <= 0)
1628 1680 {
1629 if ((m_PIDTau < 1)) 1681 PID_G = m_PIDTau + 1;
1630 { 1682 }
1631 PID_G = PID_G / m_PIDTau;
1632 }
1633
1634 if ((PID_G - m_PIDTau) <= 0)
1635 {
1636 PID_G = m_PIDTau + 1;
1637 }
1638 1683
1639 1684
1640 // Where are we, and where are we headed? 1685 // Where are we, and where are we headed?
1641 d.Vector3 pos = d.BodyGetPosition(Body); 1686 d.Vector3 pos = d.BodyGetPosition(Body);
1642 d.Vector3 vel = d.BodyGetLinearVel(Body); 1687 d.Vector3 vel = d.BodyGetLinearVel(Body);
1643 1688
1644 // determine what our target height really is based on HoverType 1689
1645 switch (m_PIDHoverType) 1690 // Non-Vehicles have a limited set of Hover options.
1646 { 1691 // determine what our target height really is based on HoverType
1647 case PIDHoverType.Absolute: 1692 switch (m_PIDHoverType)
1648 m_targetHoverHeight = m_PIDHoverHeight; 1693 {
1649 break; 1694 case PIDHoverType.Ground:
1650 case PIDHoverType.Ground: 1695 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
1651 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); 1696 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
1652 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; 1697 break;
1653 break; 1698 case PIDHoverType.GroundAndWater:
1654 case PIDHoverType.GroundAndWater: 1699 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
1655 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); 1700 m_waterHeight = _parent_scene.GetWaterLevel();
1656 m_waterHeight = _parent_scene.GetWaterLevel(); 1701 if (m_groundHeight > m_waterHeight)
1657 if (m_groundHeight > m_waterHeight) 1702 {
1658 { 1703 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
1659 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; 1704 }
1660 } 1705 else
1661 else 1706 {
1662 { 1707 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
1663 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; 1708 }
1664 } 1709 break;
1665 break; 1710
1666 case PIDHoverType.Water: 1711 } // end switch (m_PIDHoverType)
1667 m_waterHeight = _parent_scene.GetWaterLevel(); 1712
1668 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; 1713
1669 break; 1714 _target_velocity =
1670 } 1715 new PhysicsVector(0.0f, 0.0f,
1671 1716 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
1672 1717 );
1673 _target_velocity = 1718
1674 new PhysicsVector(0.0f, 0.0f, 1719 // if velocity is zero, use position control; otherwise, velocity control
1675 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) 1720
1676 ); 1721 if (_target_velocity.IsIdentical(PhysicsVector.Zero, 0.1f))
1677 1722 {
1678 // if velocity is zero, use position control; otherwise, velocity control 1723 // keep track of where we stopped. No more slippin' & slidin'
1679 1724
1680 if (_target_velocity.IsIdentical(PhysicsVector.Zero, 0.1f)) 1725 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1681 { 1726 // react to the physics scene by moving it's position.
1682 // keep track of where we stopped. No more slippin' & slidin' 1727 // Avatar to Avatar collisions
1683 1728 // Prim to avatar collisions
1684 // We only want to deactivate the PID Controller if we think we want to have our surrogate 1729
1685 // react to the physics scene by moving it's position. 1730 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
1686 // Avatar to Avatar collisions 1731 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
1687 // Prim to avatar collisions 1732 d.BodyAddForce(Body, 0, 0, fz);
1688 1733 return;
1689 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); 1734 }
1690 d.BodySetLinearVel(Body, vel.X, vel.Y, 0); 1735 else
1691 d.BodyAddForce(Body, 0, 0, fz); 1736 {
1692 return; 1737 _zeroFlag = false;
1693 } 1738
1694 else 1739 // We're flying and colliding with something
1695 { 1740 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
1696 _zeroFlag = false; 1741 }
1697 1742 }
1698 // We're flying and colliding with something 1743
1699 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); 1744 fx *= m_mass;
1700 } 1745 fy *= m_mass;
1701 } 1746 //fz *= m_mass;
1702 1747
1703 fx *= m_mass; 1748 fx += m_force.X;
1704 fy *= m_mass; 1749 fy += m_force.Y;
1705 //fz *= m_mass; 1750 fz += m_force.Z;
1706 1751
1707 fx += m_force.X; 1752 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
1708 fy += m_force.Y; 1753 if (fx != 0 || fy != 0 || fz != 0)
1709 fz += m_force.Z; 1754 {
1710 1755 //m_taintdisable = true;
1711 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); 1756 //base.RaiseOutOfBounds(Position);
1712 if (fx != 0 || fy != 0 || fz != 0) 1757 //d.BodySetLinearVel(Body, fx, fy, 0f);
1713 { 1758 if (!d.BodyIsEnabled(Body))
1714 //m_taintdisable = true; 1759 {
1715 //base.RaiseOutOfBounds(Position); 1760 // A physical body at rest on a surface will auto-disable after a while,
1716 //d.BodySetLinearVel(Body, fx, fy, 0f); 1761 // this appears to re-enable it incase the surface it is upon vanishes,
1717 if (!d.BodyIsEnabled(Body)) 1762 // and the body should fall again.
1718 { 1763 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1719 d.BodySetLinearVel(Body, 0f, 0f, 0f); 1764 d.BodySetForce(Body, 0, 0, 0);
1720 d.BodySetForce(Body, 0, 0, 0); 1765 enableBodySoft();
1721 enableBodySoft(); 1766 }
1722 } 1767
1723 1768 // 35x10 = 350n times the mass per second applied maximum.
1724 // 35x10 = 350n times the mass per second applied maximum. 1769 float nmax = 35f * m_mass;
1725 float nmax = 35f * m_mass; 1770 float nmin = -35f * m_mass;
1726 float nmin = -35f * m_mass;
1727 1771
1728 1772
1729 if (fx > nmax) 1773 if (fx > nmax)
1730 fx = nmax; 1774 fx = nmax;
1731 if (fx < nmin) 1775 if (fx < nmin)
1732 fx = nmin; 1776 fx = nmin;
1733 if (fy > nmax) 1777 if (fy > nmax)
1734 fy = nmax; 1778 fy = nmax;
1735 if (fy < nmin) 1779 if (fy < nmin)
1736 fy = nmin; 1780 fy = nmin;
1737 d.BodyAddForce(Body, fx, fy, fz); 1781 d.BodyAddForce(Body, fx, fy, fz);
1738 } 1782 }
1739 if (m_vehicle.Body == IntPtr.Zero && m_vehicle.Type != Vehicle.TYPE_NONE) 1783 }
1740 m_vehicle.Enable(Body, _parent_scene);
1741
1742 m_vehicle.Step(timestep);
1743 } 1784 }
1744 else 1785 else
1745 { 1786 { // is not physical, or is not a body or is selected
1746 // _zeroPosition = d.BodyGetPosition(Body); 1787 // _zeroPosition = d.BodyGetPosition(Body);
1747 return; 1788 return;
1748 } 1789 }
1749 } 1790 }
@@ -1757,14 +1798,22 @@ namespace OpenSim.Region.Physics.OdePlugin
1757 myrot.Y = _orientation.Y; 1798 myrot.Y = _orientation.Y;
1758 myrot.Z = _orientation.Z; 1799 myrot.Z = _orientation.Z;
1759 myrot.W = _orientation.W; 1800 myrot.W = _orientation.W;
1760 d.GeomSetQuaternion(prim_geom, ref myrot); 1801 if (Body != IntPtr.Zero)
1761 if (m_isphysical && Body != IntPtr.Zero)
1762 { 1802 {
1803 // KF: If this is a root prim do BodySet
1763 d.BodySetQuaternion(Body, ref myrot); 1804 d.BodySetQuaternion(Body, ref myrot);
1764 if (!m_angularlock.IsIdentical(new PhysicsVector(1, 1, 1), 0)) 1805 if (m_isphysical)
1765 createAMotor(m_angularlock); 1806 {
1807 if (!m_angularlock.IsIdentical(new PhysicsVector(1, 1, 1), 0))
1808 createAMotor(m_angularlock);
1809 }
1810 }
1811 else
1812 {
1813 // daughter prim, do Geom set
1814 d.GeomSetQuaternion(prim_geom, ref myrot);
1766 } 1815 }
1767 1816
1768 resetCollisionAccounting(); 1817 resetCollisionAccounting();
1769 m_taintrot = _orientation; 1818 m_taintrot = _orientation;
1770 } 1819 }
@@ -1826,7 +1875,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1826 m_log.Error("[PHYSICS]: PrimGeom dead"); 1875 m_log.Error("[PHYSICS]: PrimGeom dead");
1827 } 1876 }
1828 } 1877 }
1829 1878//Console.WriteLine("changePhysicsStatus for " + m_primName );
1830 changeadd(2f); 1879 changeadd(2f);
1831 } 1880 }
1832 if (childPrim) 1881 if (childPrim)
@@ -1904,7 +1953,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1904 mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); 1953 mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
1905 1954
1906 //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); 1955 //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical);
1907 1956//Console.WriteLine("changesize 1");
1908 CreateGeom(m_targetSpace, mesh); 1957 CreateGeom(m_targetSpace, mesh);
1909 1958
1910 1959
@@ -1912,6 +1961,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1912 else 1961 else
1913 { 1962 {
1914 _mesh = null; 1963 _mesh = null;
1964//Console.WriteLine("changesize 2");
1915 CreateGeom(m_targetSpace, _mesh); 1965 CreateGeom(m_targetSpace, _mesh);
1916 } 1966 }
1917 1967
@@ -2018,6 +2068,7 @@ namespace OpenSim.Region.Physics.OdePlugin
2018 else 2068 else
2019 { 2069 {
2020 _mesh = null; 2070 _mesh = null;
2071//Console.WriteLine("changeshape");
2021 CreateGeom(m_targetSpace, null); 2072 CreateGeom(m_targetSpace, null);
2022 } 2073 }
2023 2074
@@ -2359,7 +2410,9 @@ namespace OpenSim.Region.Physics.OdePlugin
2359 set 2410 set
2360 { 2411 {
2361 if (QuaternionIsFinite(value)) 2412 if (QuaternionIsFinite(value))
2413 {
2362 _orientation = value; 2414 _orientation = value;
2415 }
2363 else 2416 else
2364 m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object"); 2417 m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object");
2365 2418
@@ -2578,12 +2631,16 @@ namespace OpenSim.Region.Physics.OdePlugin
2578 //outofBounds = true; 2631 //outofBounds = true;
2579 } 2632 }
2580 2633
2634 float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation));
2635//Console.WriteLine("Adiff " + m_primName + " = " + Adiff);
2581 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) 2636 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
2582 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) 2637 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
2583 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) 2638 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
2584 && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01)) 2639// && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01))
2640 && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large
2585 { 2641 {
2586 _zeroFlag = true; 2642 _zeroFlag = true;
2643//Console.WriteLine("ZFT 2");
2587 m_throttleUpdates = false; 2644 m_throttleUpdates = false;
2588 } 2645 }
2589 else 2646 else
generated by cgit v1.1 at 2024-07-17 05:25:47 +0000