aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics
diff options
context:
space:
mode:
authorMelanie2010-03-15 17:12:08 +0000
committerMelanie2010-03-15 17:12:08 +0000
commit898fa637c3ef1e86d093239e4b968910b978dea5 (patch)
tree4497805d6113335b82603ec33b8845aea0e47351 /OpenSim/Region/Physics
parentCache UserLevel in ScenePresence on SP creation. Change IsAdministrator (diff)
parentMerge branch 'master' of ssh://3dhosting.de/var/git/careminster (diff)
downloadopensim-SC-898fa637c3ef1e86d093239e4b968910b978dea5.zip
opensim-SC-898fa637c3ef1e86d093239e4b968910b978dea5.tar.gz
opensim-SC-898fa637c3ef1e86d093239e4b968910b978dea5.tar.bz2
opensim-SC-898fa637c3ef1e86d093239e4b968910b978dea5.tar.xz
Merge branch 'careminster' into careminster-presence-refactor
Diffstat (limited to 'OpenSim/Region/Physics')
-rw-r--r--OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsActor.cs10
-rw-r--r--OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs10
-rw-r--r--OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs10
-rw-r--r--OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs12
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs10
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.c_comments630
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs792
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs2752
-rw-r--r--OpenSim/Region/Physics/Manager/PhysicsActor.cs12
-rw-r--r--OpenSim/Region/Physics/OdePlugin/ODECharacter.cs10
-rw-r--r--OpenSim/Region/Physics/OdePlugin/ODEPrim.cs10
-rw-r--r--OpenSim/Region/Physics/POSPlugin/POSCharacter.cs10
-rw-r--r--OpenSim/Region/Physics/POSPlugin/POSPrim.cs10
-rw-r--r--OpenSim/Region/Physics/PhysXPlugin/PhysXCharacter.cs8
-rw-r--r--OpenSim/Region/Physics/PhysXPlugin/PhysXPrim.cs2
15 files changed, 1604 insertions, 2684 deletions
diff --git a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsActor.cs b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsActor.cs
index 25bf297..5e2eeeb 100644
--- a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsActor.cs
+++ b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsActor.cs
@@ -184,17 +184,7 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin
184 { 184 {
185 185
186 } 186 }
187
188 public override void VehicleFlagsSet(int flags)
189 {
190
191 }
192
193 public override void VehicleFlagsRemove(int flags)
194 {
195 187
196 }
197
198 public override void VehicleFlags(int param, bool remove) 188 public override void VehicleFlags(int param, bool remove)
199 { 189 {
200 190
diff --git a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs
index 7d5c078..198962b 100644
--- a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs
+++ b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETCharacter.cs
@@ -361,17 +361,7 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
361 { 361 {
362 362
363 } 363 }
364
365 public override void VehicleFlagsSet(int flags)
366 {
367
368 }
369
370 public override void VehicleFlagsRemove(int flags)
371 {
372 364
373 }
374
375 public override void VehicleFlags(int param, bool remove) 365 public override void VehicleFlags(int param, bool remove)
376 { 366 {
377 367
diff --git a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs
index 1f3e031..920ed96 100644
--- a/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs
+++ b/OpenSim/Region/Physics/BulletDotNETPlugin/BulletDotNETPrim.cs
@@ -396,17 +396,7 @@ namespace OpenSim.Region.Physics.BulletDotNETPlugin
396 { 396 {
397 //TODO: 397 //TODO:
398 } 398 }
399
400 public override void VehicleFlagsSet(int flags)
401 {
402
403 }
404
405 public override void VehicleFlagsRemove(int flags)
406 {
407 399
408 }
409
410 public override void VehicleFlags(int param, bool remove) 400 public override void VehicleFlags(int param, bool remove)
411 { 401 {
412 402
diff --git a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
index 17f99f5..f4245b6 100644
--- a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
+++ b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
@@ -996,22 +996,12 @@ namespace OpenSim.Region.Physics.BulletXPlugin
996 { 996 {
997 997
998 } 998 }
999
1000 public override void VehicleFlagsSet(int flags)
1001 {
1002
1003 }
1004
1005 public override void VehicleFlagsRemove(int flags)
1006 {
1007
1008 }
1009 999
1010 public override void VehicleFlags(int param, bool remove) 1000 public override void VehicleFlags(int param, bool remove)
1011 { 1001 {
1012 1002
1013 } 1003 }
1014 1004
1015 public override void SetVolumeDetect(int param) 1005 public override void SetVolumeDetect(int param)
1016 { 1006 {
1017 1007
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs
index 6dadbe5..8e87ad9 100644
--- a/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODECharacter.cs
@@ -738,16 +738,6 @@ namespace OpenSim.Region.Physics.OdePlugin
738 { 738 {
739 } 739 }
740 740
741 public override void VehicleFlagsSet(int flags)
742 {
743
744 }
745
746 public override void VehicleFlagsRemove(int flags)
747 {
748
749 }
750
751 public override void SetVolumeDetect(int param) 741 public override void SetVolumeDetect(int param)
752 { 742 {
753 743
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.c_comments b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.c_comments
deleted file mode 100644
index 1060aa6..0000000
--- a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.c_comments
+++ /dev/null
@@ -1,630 +0,0 @@
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.
13 *
14 * Copyright (c) Contributors, http://opensimulator.org/
15 * See CONTRIBUTORS.TXT for a full list of copyright holders.
16 *
17 * Redistribution and use in source and binary forms, with or without
18 * modification, are permitted provided that the following conditions are met:
19 * * Redistributions of source code must retain the above copyright
20 * notice, this list of conditions and the following disclaimer.
21 * * Redistributions in binary form must reproduce the above copyright
22 * notice, this list of conditions and the following disclaimer in the
23 * documentation and/or other materials provided with the distribution.
24 * * Neither the name of the OpenSimulator Project nor the
25 * names of its contributors may be used to endorse or promote products
26 * derived from this software without specific prior written permission.
27 *
28 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
29 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
30 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
31 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
32 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
33 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
34 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
35 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
36 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
37 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
38 */
39
40using System;
41using System.Collections.Generic;
42using System.Reflection;
43using System.Runtime.InteropServices;
44using log4net;
45using OpenMetaverse;
46using Ode.NET;
47using OpenSim.Framework;
48using OpenSim.Region.Physics.Manager;
49
50namespace OpenSim.Region.Physics.OdePlugin
51{
52 public class ODEDynamics
53 {
54 public Vehicle Type
55 {
56 get { return m_type; }
57 }
58
59 public IntPtr Body
60 {
61 get { return m_body; }
62 }
63
64 private int frcount = 0; // Used to limit dynamics debug output to
65 // every 100th frame
66
67 // private OdeScene m_parentScene = null;
68 private IntPtr m_body = IntPtr.Zero;
69 private IntPtr m_jointGroup = IntPtr.Zero;
70 private IntPtr m_aMotor = IntPtr.Zero;
71
72
73 // Vehicle properties
74 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
75 // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
76 private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings:
77 // HOVER_TERRAIN_ONLY
78 // HOVER_GLOBAL_HEIGHT
79 // NO_DEFLECTION_UP
80 // HOVER_WATER_ONLY
81 // HOVER_UP_ONLY
82 // LIMIT_MOTOR_UP
83 // LIMIT_ROLL_ONLY
84
85 // Linear properties
86 private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time
87 private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL
88 private Vector3 m_dir = Vector3.Zero; // velocity applied to body
89 private Vector3 m_linearFrictionTimescale = Vector3.Zero;
90 private float m_linearMotorDecayTimescale = 0;
91 private float m_linearMotorTimescale = 0;
92 private Vector3 m_lastLinearVelocityVector = Vector3.Zero;
93 // private bool m_LinearMotorSetLastFrame = false;
94 // private Vector3 m_linearMotorOffset = Vector3.Zero;
95
96 //Angular properties
97 private Vector3 m_angularMotorDirection = Vector3.Zero;
98 private Vector3 m_angularMotorDirectionLASTSET = Vector3.Zero;
99 private Vector3 m_angularFrictionTimescale = Vector3.Zero;
100 private float m_angularMotorDecayTimescale = 0;
101 private float m_angularMotorTimescale = 0;
102 private Vector3 m_lastAngularVelocityVector = Vector3.Zero;
103
104 //Deflection properties
105 // private float m_angularDeflectionEfficiency = 0;
106 // private float m_angularDeflectionTimescale = 0;
107 // private float m_linearDeflectionEfficiency = 0;
108 // private float m_linearDeflectionTimescale = 0;
109
110 //Banking properties
111 // private float m_bankingEfficiency = 0;
112 // private float m_bankingMix = 0;
113 // private float m_bankingTimescale = 0;
114
115 //Hover and Buoyancy properties
116 private float m_VhoverHeight = 0f;
117 private float m_VhoverEfficiency = 0f;
118 private float m_VhoverTimescale = 0f;
119 private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height
120 private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle.
121 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
122 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
123 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
124
125 //Attractor properties
126 private float m_verticalAttractionEfficiency = 0;
127 private float m_verticalAttractionTimescale = 0;
128
129
130
131
132
133 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
134 {
135 switch (pParam)
136 {
137 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
138 if (pValue < 0.01f) pValue = 0.01f;
139 // m_angularDeflectionEfficiency = pValue;
140 break;
141 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
142 if (pValue < 0.01f) pValue = 0.01f;
143 // m_angularDeflectionTimescale = pValue;
144 break;
145 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
146 if (pValue < 0.01f) pValue = 0.01f;
147 m_angularMotorDecayTimescale = pValue;
148 break;
149 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
150 if (pValue < 0.01f) pValue = 0.01f;
151 m_angularMotorTimescale = pValue;
152 break;
153 case Vehicle.BANKING_EFFICIENCY:
154 if (pValue < 0.01f) pValue = 0.01f;
155 // m_bankingEfficiency = pValue;
156 break;
157 case Vehicle.BANKING_MIX:
158 if (pValue < 0.01f) pValue = 0.01f;
159 // m_bankingMix = pValue;
160 break;
161 case Vehicle.BANKING_TIMESCALE:
162 if (pValue < 0.01f) pValue = 0.01f;
163 // m_bankingTimescale = pValue;
164 break;
165 case Vehicle.BUOYANCY:
166 if (pValue < -1f) pValue = -1f;
167 if (pValue > 1f) pValue = 1f;
168 m_VehicleBuoyancy = pValue;
169 break;
170 case Vehicle.HOVER_EFFICIENCY:
171 if (pValue < 0f) pValue = 0f;
172 if (pValue > 1f) pValue = 1f;
173 m_VhoverEfficiency = pValue;
174 break;
175 case Vehicle.HOVER_HEIGHT:
176 m_VhoverHeight = pValue;
177 break;
178 case Vehicle.HOVER_TIMESCALE:
179 if (pValue < 0.01f) pValue = 0.01f;
180 m_VhoverTimescale = pValue;
181 break;
182 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
183 if (pValue < 0.01f) pValue = 0.01f;
184 // m_linearDeflectionEfficiency = pValue;
185 break;
186 case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
187 if (pValue < 0.01f) pValue = 0.01f;
188 // m_linearDeflectionTimescale = pValue;
189 break;
190 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
191 if (pValue < 0.01f) pValue = 0.01f;
192 m_linearMotorDecayTimescale = pValue;
193 break;
194 case Vehicle.LINEAR_MOTOR_TIMESCALE:
195 if (pValue < 0.01f) pValue = 0.01f;
196 m_linearMotorTimescale = pValue;
197 break;
198 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
199 if (pValue < 0.0f) pValue = 0.0f;
200 if (pValue > 1.0f) pValue = 1.0f;
201 m_verticalAttractionEfficiency = pValue;
202 break;
203 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
204 if (pValue < 0.01f) pValue = 0.01f;
205 m_verticalAttractionTimescale = pValue;
206 break;
207
208 // These are vector properties but the engine lets you use a single float value to
209 // set all of the components to the same value
210 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
211 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
212 break;
213 case Vehicle.ANGULAR_MOTOR_DIRECTION:
214 m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
215 m_angularMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue);
216 break;
217 case Vehicle.LINEAR_FRICTION_TIMESCALE:
218 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
219 break;
220 case Vehicle.LINEAR_MOTOR_DIRECTION:
221 m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
222 m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue);
223 break;
224 case Vehicle.LINEAR_MOTOR_OFFSET:
225 // m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
226 break;
227
228 }
229
230 }//end ProcessFloatVehicleParam
231
232 internal void ProcessVectorVehicleParam(Vehicle pParam, PhysicsVector pValue)
233 {
234 switch (pParam)
235 {
236 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
237 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
238 break;
239 case Vehicle.ANGULAR_MOTOR_DIRECTION:
240 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
241 m_angularMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z);
242 break;
243 case Vehicle.LINEAR_FRICTION_TIMESCALE:
244 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
245 break;
246 case Vehicle.LINEAR_MOTOR_DIRECTION:
247 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
248 m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z);
249 break;
250 case Vehicle.LINEAR_MOTOR_OFFSET:
251 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
252 break;
253 }
254
255 }//end ProcessVectorVehicleParam
256
257 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
258 {
259 switch (pParam)
260 {
261 case Vehicle.REFERENCE_FRAME:
262 // m_referenceFrame = pValue;
263 break;
264 }
265
266 }//end ProcessRotationVehicleParam
267
268 internal void ProcessTypeChange(Vehicle pType)
269 {
270Console.WriteLine("ProcessTypeChange to " + pType);
271
272 // Set Defaults For Type
273 m_type = pType;
274 switch (pType)
275 {
276 case Vehicle.TYPE_SLED:
277 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
278 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
279 m_linearMotorDirection = Vector3.Zero;
280 m_linearMotorTimescale = 1000;
281 m_linearMotorDecayTimescale = 120;
282 m_angularMotorDirection = Vector3.Zero;
283 m_angularMotorTimescale = 1000;
284 m_angularMotorDecayTimescale = 120;
285 m_VhoverHeight = 0;
286 m_VhoverEfficiency = 1;
287 m_VhoverTimescale = 10;
288 m_VehicleBuoyancy = 0;
289 // m_linearDeflectionEfficiency = 1;
290 // m_linearDeflectionTimescale = 1;
291 // m_angularDeflectionEfficiency = 1;
292 // m_angularDeflectionTimescale = 1000;
293 // m_bankingEfficiency = 0;
294 // m_bankingMix = 1;
295 // m_bankingTimescale = 10;
296 // m_referenceFrame = Quaternion.Identity;
297 m_flags &=
298 ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
299 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
300 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
301 break;
302 case Vehicle.TYPE_CAR:
303 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
304 m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
305 m_linearMotorDirection = Vector3.Zero;
306 m_linearMotorTimescale = 1;
307 m_linearMotorDecayTimescale = 60;
308 m_angularMotorDirection = Vector3.Zero;
309 m_angularMotorTimescale = 1;
310 m_angularMotorDecayTimescale = 0.8f;
311 m_VhoverHeight = 0;
312 m_VhoverEfficiency = 0;
313 m_VhoverTimescale = 1000;
314 m_VehicleBuoyancy = 0;
315 // // m_linearDeflectionEfficiency = 1;
316 // // m_linearDeflectionTimescale = 2;
317 // // m_angularDeflectionEfficiency = 0;
318 // m_angularDeflectionTimescale = 10;
319 m_verticalAttractionEfficiency = 1;
320 m_verticalAttractionTimescale = 10;
321 // m_bankingEfficiency = -0.2f;
322 // m_bankingMix = 1;
323 // m_bankingTimescale = 1;
324 // m_referenceFrame = Quaternion.Identity;
325 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
326 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY |
327 VehicleFlag.LIMIT_MOTOR_UP);
328 break;
329 case Vehicle.TYPE_BOAT:
330 m_linearFrictionTimescale = new Vector3(10, 3, 2);
331 m_angularFrictionTimescale = new Vector3(10,10,10);
332 m_linearMotorDirection = Vector3.Zero;
333 m_linearMotorTimescale = 5;
334 m_linearMotorDecayTimescale = 60;
335 m_angularMotorDirection = Vector3.Zero;
336 m_angularMotorTimescale = 4;
337 m_angularMotorDecayTimescale = 4;
338 m_VhoverHeight = 0;
339 m_VhoverEfficiency = 0.5f;
340 m_VhoverTimescale = 2;
341 m_VehicleBuoyancy = 1;
342 // m_linearDeflectionEfficiency = 0.5f;
343 // m_linearDeflectionTimescale = 3;
344 // m_angularDeflectionEfficiency = 0.5f;
345 // m_angularDeflectionTimescale = 5;
346 m_verticalAttractionEfficiency = 0.5f;
347 m_verticalAttractionTimescale = 5;
348 // m_bankingEfficiency = -0.3f;
349 // m_bankingMix = 0.8f;
350 // m_bankingTimescale = 1;
351 // m_referenceFrame = Quaternion.Identity;
352 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY |
353 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
354 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY |
355 VehicleFlag.LIMIT_MOTOR_UP);
356 break;
357 case Vehicle.TYPE_AIRPLANE:
358 m_linearFrictionTimescale = new Vector3(200, 10, 5);
359 m_angularFrictionTimescale = new Vector3(20, 20, 20);
360 m_linearMotorDirection = Vector3.Zero;
361 m_linearMotorTimescale = 2;
362 m_linearMotorDecayTimescale = 60;
363 m_angularMotorDirection = Vector3.Zero;
364 m_angularMotorTimescale = 4;
365 m_angularMotorDecayTimescale = 4;
366 m_VhoverHeight = 0;
367 m_VhoverEfficiency = 0.5f;
368 m_VhoverTimescale = 1000;
369 m_VehicleBuoyancy = 0;
370 // m_linearDeflectionEfficiency = 0.5f;
371 // m_linearDeflectionTimescale = 3;
372 // m_angularDeflectionEfficiency = 1;
373 // m_angularDeflectionTimescale = 2;
374 m_verticalAttractionEfficiency = 0.9f;
375 m_verticalAttractionTimescale = 2;
376 // m_bankingEfficiency = 1;
377 // m_bankingMix = 0.7f;
378 // m_bankingTimescale = 2;
379 // m_referenceFrame = Quaternion.Identity;
380 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
381 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
382 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
383 break;
384 case Vehicle.TYPE_BALLOON:
385 m_linearFrictionTimescale = new Vector3(5, 5, 5);
386 m_angularFrictionTimescale = new Vector3(10, 10, 10);
387 m_linearMotorDirection = Vector3.Zero;
388 m_linearMotorTimescale = 5;
389 m_linearMotorDecayTimescale = 60;
390 m_angularMotorDirection = Vector3.Zero;
391 m_angularMotorTimescale = 6;
392 m_angularMotorDecayTimescale = 10;
393 m_VhoverHeight = 5;
394 m_VhoverEfficiency = 0.8f;
395 m_VhoverTimescale = 10;
396 m_VehicleBuoyancy = 1;
397 // m_linearDeflectionEfficiency = 0;
398 // m_linearDeflectionTimescale = 5;
399 // m_angularDeflectionEfficiency = 0;
400 // m_angularDeflectionTimescale = 5;
401 m_verticalAttractionEfficiency = 1;
402 m_verticalAttractionTimescale = 1000;
403 // m_bankingEfficiency = 0;
404 // m_bankingMix = 0.7f;
405 // m_bankingTimescale = 5;
406 // m_referenceFrame = Quaternion.Identity;
407 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
408 VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
409 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
410 break;
411
412 }
413 }//end SetDefaultsForType
414
415 internal void Enable(IntPtr pBody, OdeScene pParentScene)
416 {
417//Console.WriteLine("Enable m_type=" + m_type + " m_VehicleBuoyancy=" + m_VehicleBuoyancy);
418 if (m_type == Vehicle.TYPE_NONE)
419 return;
420
421 m_body = pBody;
422 //KF: This used to set up the linear and angular joints
423 }
424
425 internal void Step(float pTimestep, OdeScene pParentScene)
426 {
427 if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
428 return;
429 frcount++; // used to limit debug comment output
430 if (frcount > 100)
431 frcount = 0;
432
433 MoveLinear(pTimestep, pParentScene);
434 MoveAngular(pTimestep);
435 }// end Step
436
437 private void MoveLinear(float pTimestep, OdeScene _pParentScene)
438 {
439 if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant
440 {
441 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
442
443 // add drive to body
444 Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep);
445 m_lastLinearVelocityVector += (addAmount*10); // lastLinearVelocityVector is the current body velocity vector?
446
447 // This will work temporarily, but we really need to compare speed on an axis
448 // KF: Limit body velocity to applied velocity?
449 if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X))
450 m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X;
451 if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y))
452 m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y;
453 if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z))
454 m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z;
455
456 // decay applied velocity
457 Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep)));
458 //Console.WriteLine("decay: " + decayfraction);
459 m_linearMotorDirection -= m_linearMotorDirection * decayfraction;
460 //Console.WriteLine("actual: " + m_linearMotorDirection);
461 }
462 else
463 { // requested is not significant
464 // if what remains of applied is small, zero it.
465 if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f))
466 m_lastLinearVelocityVector = Vector3.Zero;
467 }
468
469
470 // convert requested object velocity to world-referenced vector
471 m_dir = m_lastLinearVelocityVector;
472 d.Quaternion rot = d.BodyGetQuaternion(Body);
473 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
474 m_dir *= rotq; // apply obj rotation to velocity vector
475
476 // add Gravity andBuoyancy
477 // KF: So far I have found no good method to combine a script-requested
478 // .Z velocity and gravity. Therefore only 0g will used script-requested
479 // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only.
480 Vector3 grav = Vector3.Zero;
481 if(m_VehicleBuoyancy < 1.0f)
482 {
483 // There is some gravity, make a gravity force vector
484 // that is applied after object velocity.
485 d.Mass objMass;
486 d.BodyGetMass(Body, out objMass);
487 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
488 grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy);
489 // Preserve the current Z velocity
490 d.Vector3 vel_now = d.BodyGetLinearVel(Body);
491 m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity
492 } // else its 1.0, no gravity.
493
494 // Check if hovering
495 if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
496 {
497 // We should hover, get the target height
498 d.Vector3 pos = d.BodyGetPosition(Body);
499 if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY)
500 {
501 m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight;
502 }
503 else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY)
504 {
505 m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
506 }
507 else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT)
508 {
509 m_VhoverTargetHeight = m_VhoverHeight;
510 }
511
512 if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY)
513 {
514 // If body is aready heigher, use its height as target height
515 if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
516 }
517
518// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
519// m_VhoverTimescale = 0f; // time to acheive height
520// pTimestep is time since last frame,in secs
521 float herr0 = pos.Z - m_VhoverTargetHeight;
522//if(frcount == 0) Console.WriteLine("herr0=" + herr0);
523 // Replace Vertical speed with correction figure if significant
524 if(Math.Abs(herr0) > 0.01f )
525 {
526 d.Mass objMass;
527 d.BodyGetMass(Body, out objMass);
528 m_dir.Z = - ( (herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
529 // m_VhoverEfficiency is not yet implemented
530 }
531 else
532 {
533 m_dir.Z = 0f;
534 }
535 }
536
537 // Apply velocity
538 d.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z);
539//if(frcount == 0) Console.WriteLine("Move " + Body + ":"+ m_dir.X + " " + m_dir.Y + " " + m_dir.Z);
540 // apply gravity force
541 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
542//if(frcount == 0) Console.WriteLine("Force " + Body + ":" + grav.X + " " + grav.Y + " " + grav.Z);
543
544
545 // apply friction
546 Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);
547 m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
548 } // end MoveLinear()
549
550 private void MoveAngular(float pTimestep)
551 {
552
553 // m_angularMotorDirection is the latest value from the script, and is decayed here
554 // m_angularMotorDirectionLASTSET is the latest value from the script
555 // m_lastAngularVelocityVector is what is being applied to the Body, varied up and down here
556
557 if (!m_angularMotorDirection.ApproxEquals(Vector3.Zero, 0.01f))
558 {
559 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
560 // ramp up to new value
561 Vector3 addAmount = m_angularMotorDirection / (m_angularMotorTimescale / pTimestep);
562 m_lastAngularVelocityVector += (addAmount * 10f);
563//if(frcount == 0) Console.WriteLine("add: " + addAmount);
564
565 // limit applied value to what was set by script
566 // This will work temporarily, but we really need to compare speed on an axis
567 if (Math.Abs(m_lastAngularVelocityVector.X) > Math.Abs(m_angularMotorDirectionLASTSET.X))
568 m_lastAngularVelocityVector.X = m_angularMotorDirectionLASTSET.X;
569 if (Math.Abs(m_lastAngularVelocityVector.Y) > Math.Abs(m_angularMotorDirectionLASTSET.Y))
570 m_lastAngularVelocityVector.Y = m_angularMotorDirectionLASTSET.Y;
571 if (Math.Abs(m_lastAngularVelocityVector.Z) > Math.Abs(m_angularMotorDirectionLASTSET.Z))
572 m_lastAngularVelocityVector.Z = m_angularMotorDirectionLASTSET.Z;
573
574 // decay the requested value
575 Vector3 decayfraction = ((Vector3.One / (m_angularMotorDecayTimescale / pTimestep)));
576 //Console.WriteLine("decay: " + decayfraction);
577 m_angularMotorDirection -= m_angularMotorDirection * decayfraction;
578 //Console.WriteLine("actual: " + m_linearMotorDirection);
579 }
580 // KF: m_lastAngularVelocityVector is rotational speed in rad/sec ?
581
582 // Vertical attractor section
583
584// d.Mass objMass;
585// d.BodyGetMass(Body, out objMass);
586// float servo = 100f * objMass.mass * m_verticalAttractionEfficiency / (m_verticalAttractionTimescale * pTimestep);
587 float servo = 0.1f * m_verticalAttractionEfficiency / (m_verticalAttractionTimescale * pTimestep);
588 // get present body rotation
589 d.Quaternion rot = d.BodyGetQuaternion(Body);
590 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
591 // make a vector pointing up
592 Vector3 verterr = Vector3.Zero;
593 verterr.Z = 1.0f;
594 // rotate it to Body Angle
595 verterr = verterr * rotq;
596 // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1.
597 // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go
598 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
599 if (verterr.Z < 0.0f)
600 {
601 verterr.X = 2.0f - verterr.X;
602 verterr.Y = 2.0f - verterr.Y;
603 }
604 // Error is 0 (no error) to +/- 2 (max error)
605 // scale it by servo
606 verterr = verterr * servo;
607
608 // rotate to object frame
609 // verterr = verterr * rotq;
610
611 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
612 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
613 m_lastAngularVelocityVector.X += verterr.Y;
614 m_lastAngularVelocityVector.Y -= verterr.X;
615/*
616if(frcount == 0)
617 {
618// Console.WriteLine("AngleMotor " + m_lastAngularVelocityVector);
619 Console.WriteLine(String.Format("VA Body:{0} servo:{1} err:<{2},{3},{4}> VAE:{5}",
620 Body, servo, verterr.X, verterr.Y, verterr.Z, m_verticalAttractionEfficiency));
621 }
622 */
623 d.BodySetAngularVel (Body, m_lastAngularVelocityVector.X, m_lastAngularVelocityVector.Y, m_lastAngularVelocityVector.Z);
624 // apply friction
625 Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep);
626 m_lastAngularVelocityVector -= m_lastAngularVelocityVector * decayamount;
627
628 } //end MoveAngular
629 }
630}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs
deleted file mode 100644
index 55d6945..0000000
--- a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs
+++ /dev/null
@@ -1,792 +0,0 @@
1/*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 *
27 * Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces
28 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
29 * ODEPrim.cs contains methods dealing with Prim editing, Prim
30 * characteristics and Kinetic motion.
31 * ODEDynamics.cs contains methods dealing with Prim Physical motion
32 * (dynamics) and the associated settings. Old Linear and angular
33 * motors for dynamic motion have been replace with MoveLinear()
34 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
35 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
36 * switch between 'VEHICLE' parameter use and general dynamics
37 * settings use.
38 *
39 */
40
41/* Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces
42 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
43 * ODEPrim.cs contains methods dealing with Prim editing, Prim
44 * characteristics and Kinetic motion.
45 * ODEDynamics.cs contains methods dealing with Prim Physical motion
46 * (dynamics) and the associated settings. Old Linear and angular
47 * motors for dynamic motion have been replace with MoveLinear()
48 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
49 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
50 * switch between 'VEHICLE' parameter use and general dynamics
51 * settings use.
52 */
53
54using System;
55using System.Collections.Generic;
56using System.Reflection;
57using System.Runtime.InteropServices;
58using log4net;
59using OpenMetaverse;
60using Ode.NET;
61using OpenSim.Framework;
62using OpenSim.Region.Physics.Manager;
63
64namespace OpenSim.Region.Physics.OdePlugin
65{
66 public class ODEDynamics
67 {
68 public Vehicle Type
69 {
70 get { return m_type; }
71 }
72
73 public IntPtr Body
74 {
75 get { return m_body; }
76 }
77
78 private int frcount = 0; // Used to limit dynamics debug output to
79 // every 100th frame
80
81 // private OdeScene m_parentScene = null;
82 private IntPtr m_body = IntPtr.Zero;
83// private IntPtr m_jointGroup = IntPtr.Zero;
84// private IntPtr m_aMotor = IntPtr.Zero;
85
86 // Vehicle properties
87 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
88 // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
89 private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings:
90 // HOVER_TERRAIN_ONLY
91 // HOVER_GLOBAL_HEIGHT
92 // NO_DEFLECTION_UP
93 // HOVER_WATER_ONLY
94 // HOVER_UP_ONLY
95 // LIMIT_MOTOR_UP
96 // LIMIT_ROLL_ONLY
97
98 // Linear properties
99 private Vector3 m_linearMotorDirection = Vector3.Zero; // (was m_linearMotorDirectionLASTSET) the (local) Velocity
100 //requested by LSL
101 private float m_linearMotorTimescale = 0; // Motor Attack rate set by LSL
102 private float m_linearMotorDecayTimescale = 0; // Motor Decay rate set by LSL
103 private Vector3 m_linearFrictionTimescale = Vector3.Zero; // General Friction set by LSL
104
105 private Vector3 m_lLinMotorDVel = Vector3.Zero; // decayed motor
106 private Vector3 m_lLinObjectVel = Vector3.Zero; // local frame object velocity
107 private Vector3 m_wLinObjectVel = Vector3.Zero; // world frame object velocity
108
109 //Angular properties
110 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
111
112 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
113 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
114 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
115
116 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
117// private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity
118 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
119
120 //Deflection properties
121 // private float m_angularDeflectionEfficiency = 0;
122 // private float m_angularDeflectionTimescale = 0;
123 // private float m_linearDeflectionEfficiency = 0;
124 // private float m_linearDeflectionTimescale = 0;
125
126 //Banking properties
127 // private float m_bankingEfficiency = 0;
128 // private float m_bankingMix = 0;
129 // private float m_bankingTimescale = 0;
130
131 //Hover and Buoyancy properties
132 private float m_VhoverHeight = 0f;
133// private float m_VhoverEfficiency = 0f;
134 private float m_VhoverTimescale = 0f;
135 private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height
136 private float m_VehicleBuoyancy = 0f; // Set by VEHICLE_BUOYANCY, for a vehicle.
137 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
138 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
139 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
140
141 //Attractor properties
142 private float m_verticalAttractionEfficiency = 1.0f; // damped
143 private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor.
144
145
146
147
148
149 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
150 {
151 switch (pParam)
152 {
153 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
154 if (pValue < 0.01f) pValue = 0.01f;
155 // m_angularDeflectionEfficiency = pValue;
156 break;
157 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
158 if (pValue < 0.01f) pValue = 0.01f;
159 // m_angularDeflectionTimescale = pValue;
160 break;
161 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
162 if (pValue < 0.01f) pValue = 0.01f;
163 m_angularMotorDecayTimescale = pValue;
164 break;
165 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
166 if (pValue < 0.01f) pValue = 0.01f;
167 m_angularMotorTimescale = pValue;
168 break;
169 case Vehicle.BANKING_EFFICIENCY:
170 if (pValue < 0.01f) pValue = 0.01f;
171 // m_bankingEfficiency = pValue;
172 break;
173 case Vehicle.BANKING_MIX:
174 if (pValue < 0.01f) pValue = 0.01f;
175 // m_bankingMix = pValue;
176 break;
177 case Vehicle.BANKING_TIMESCALE:
178 if (pValue < 0.01f) pValue = 0.01f;
179 // m_bankingTimescale = pValue;
180 break;
181 case Vehicle.BUOYANCY:
182 if (pValue < -1f) pValue = -1f;
183 if (pValue > 1f) pValue = 1f;
184 m_VehicleBuoyancy = pValue;
185 break;
186// case Vehicle.HOVER_EFFICIENCY:
187// if (pValue < 0f) pValue = 0f;
188// if (pValue > 1f) pValue = 1f;
189// m_VhoverEfficiency = pValue;
190// break;
191 case Vehicle.HOVER_HEIGHT:
192 m_VhoverHeight = pValue;
193 break;
194 case Vehicle.HOVER_TIMESCALE:
195 if (pValue < 0.01f) pValue = 0.01f;
196 m_VhoverTimescale = pValue;
197 break;
198 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
199 if (pValue < 0.01f) pValue = 0.01f;
200 // m_linearDeflectionEfficiency = pValue;
201 break;
202 case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
203 if (pValue < 0.01f) pValue = 0.01f;
204 // m_linearDeflectionTimescale = pValue;
205 break;
206 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
207 if (pValue < 0.01f) pValue = 0.01f;
208 m_linearMotorDecayTimescale = pValue;
209 break;
210 case Vehicle.LINEAR_MOTOR_TIMESCALE:
211 if (pValue < 0.01f) pValue = 0.01f;
212 m_linearMotorTimescale = pValue;
213 break;
214 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
215 if (pValue < 0.1f) pValue = 0.1f; // Less goes unstable
216 if (pValue > 1.0f) pValue = 1.0f;
217 m_verticalAttractionEfficiency = pValue;
218 break;
219 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
220 if (pValue < 0.01f) pValue = 0.01f;
221 m_verticalAttractionTimescale = pValue;
222 break;
223
224 // These are vector properties but the engine lets you use a single float value to
225 // set all of the components to the same value
226 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
227 if (pValue > 30f) pValue = 30f;
228 if (pValue < 0.1f) pValue = 0.1f;
229 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
230 break;
231 case Vehicle.ANGULAR_MOTOR_DIRECTION:
232 m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
233 UpdateAngDecay();
234 break;
235 case Vehicle.LINEAR_FRICTION_TIMESCALE:
236 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
237 break;
238 case Vehicle.LINEAR_MOTOR_DIRECTION:
239 m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
240 UpdateLinDecay();
241 break;
242 case Vehicle.LINEAR_MOTOR_OFFSET:
243 // m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
244 break;
245
246 }
247
248 }//end ProcessFloatVehicleParam
249
250 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
251 {
252 switch (pParam)
253 {
254 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
255 if (pValue.X > 30f) pValue.X = 30f;
256 if (pValue.X < 0.1f) pValue.X = 0.1f;
257 if (pValue.Y > 30f) pValue.Y = 30f;
258 if (pValue.Y < 0.1f) pValue.Y = 0.1f;
259 if (pValue.Z > 30f) pValue.Z = 30f;
260 if (pValue.Z < 0.1f) pValue.Z = 0.1f;
261 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
262 break;
263 case Vehicle.ANGULAR_MOTOR_DIRECTION:
264 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
265 // Limit requested angular speed to 2 rps= 4 pi rads/sec
266 if(m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f;
267 if(m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f;
268 if(m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f;
269 if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f;
270 if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f;
271 if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f;
272 UpdateAngDecay();
273 break;
274 case Vehicle.LINEAR_FRICTION_TIMESCALE:
275 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
276 break;
277 case Vehicle.LINEAR_MOTOR_DIRECTION:
278 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); // velocity requested by LSL, for max limiting
279 UpdateLinDecay();
280 break;
281 case Vehicle.LINEAR_MOTOR_OFFSET:
282 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
283 break;
284 }
285
286 }//end ProcessVectorVehicleParam
287
288 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
289 {
290 switch (pParam)
291 {
292 case Vehicle.REFERENCE_FRAME:
293 // m_referenceFrame = pValue;
294 break;
295 }
296
297 }//end ProcessRotationVehicleParam
298
299 internal void ProcessFlagsVehicleSet(int flags)
300 {
301 m_flags |= (VehicleFlag)flags;
302 }
303
304 internal void ProcessFlagsVehicleRemove(int flags)
305 {
306 m_flags &= ~((VehicleFlag)flags);
307 }
308
309 internal void ProcessTypeChange(Vehicle pType)
310 {
311 // Set Defaults For Type
312 m_type = pType;
313 switch (pType)
314 {
315 case Vehicle.TYPE_SLED:
316 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
317 m_angularFrictionTimescale = new Vector3(30, 30, 30);
318// m_lLinMotorVel = Vector3.Zero;
319 m_linearMotorTimescale = 1000;
320 m_linearMotorDecayTimescale = 120;
321 m_angularMotorDirection = Vector3.Zero;
322 m_angularMotorDVel = Vector3.Zero;
323 m_angularMotorTimescale = 1000;
324 m_angularMotorDecayTimescale = 120;
325 m_VhoverHeight = 0;
326// m_VhoverEfficiency = 1;
327 m_VhoverTimescale = 10;
328 m_VehicleBuoyancy = 0;
329 // m_linearDeflectionEfficiency = 1;
330 // m_linearDeflectionTimescale = 1;
331 // m_angularDeflectionEfficiency = 1;
332 // m_angularDeflectionTimescale = 1000;
333 // m_bankingEfficiency = 0;
334 // m_bankingMix = 1;
335 // m_bankingTimescale = 10;
336 // m_referenceFrame = Quaternion.Identity;
337 m_flags &=
338 ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
339 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
340 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
341 break;
342 case Vehicle.TYPE_CAR:
343 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
344 m_angularFrictionTimescale = new Vector3(30, 30, 30); // was 1000, but sl max frict time is 30.
345// m_lLinMotorVel = Vector3.Zero;
346 m_linearMotorTimescale = 1;
347 m_linearMotorDecayTimescale = 60;
348 m_angularMotorDirection = Vector3.Zero;
349 m_angularMotorDVel = Vector3.Zero;
350 m_angularMotorTimescale = 1;
351 m_angularMotorDecayTimescale = 0.8f;
352 m_VhoverHeight = 0;
353// m_VhoverEfficiency = 0;
354 m_VhoverTimescale = 1000;
355 m_VehicleBuoyancy = 0;
356 // // m_linearDeflectionEfficiency = 1;
357 // // m_linearDeflectionTimescale = 2;
358 // // m_angularDeflectionEfficiency = 0;
359 // m_angularDeflectionTimescale = 10;
360 m_verticalAttractionEfficiency = 1f;
361 m_verticalAttractionTimescale = 10f;
362 // m_bankingEfficiency = -0.2f;
363 // m_bankingMix = 1;
364 // m_bankingTimescale = 1;
365 // m_referenceFrame = Quaternion.Identity;
366 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
367 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY |
368 VehicleFlag.LIMIT_MOTOR_UP);
369 break;
370 case Vehicle.TYPE_BOAT:
371 m_linearFrictionTimescale = new Vector3(10, 3, 2);
372 m_angularFrictionTimescale = new Vector3(10,10,10);
373// m_lLinMotorVel = Vector3.Zero;
374 m_linearMotorTimescale = 5;
375 m_linearMotorDecayTimescale = 60;
376 m_angularMotorDirection = Vector3.Zero;
377 m_angularMotorDVel = Vector3.Zero;
378 m_angularMotorTimescale = 4;
379 m_angularMotorDecayTimescale = 4;
380 m_VhoverHeight = 0;
381// m_VhoverEfficiency = 0.5f;
382 m_VhoverTimescale = 2;
383 m_VehicleBuoyancy = 1;
384 // m_linearDeflectionEfficiency = 0.5f;
385 // m_linearDeflectionTimescale = 3;
386 // m_angularDeflectionEfficiency = 0.5f;
387 // m_angularDeflectionTimescale = 5;
388 m_verticalAttractionEfficiency = 0.5f;
389 m_verticalAttractionTimescale = 5f;
390 // m_bankingEfficiency = -0.3f;
391 // m_bankingMix = 0.8f;
392 // m_bankingTimescale = 1;
393 // m_referenceFrame = Quaternion.Identity;
394 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY |
395 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
396 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY |
397 VehicleFlag.LIMIT_MOTOR_UP);
398 break;
399 case Vehicle.TYPE_AIRPLANE:
400 m_linearFrictionTimescale = new Vector3(200, 10, 5);
401 m_angularFrictionTimescale = new Vector3(20, 20, 20);
402// m_lLinMotorVel = Vector3.Zero;
403 m_linearMotorTimescale = 2;
404 m_linearMotorDecayTimescale = 60;
405 m_angularMotorDirection = Vector3.Zero;
406 m_angularMotorDVel = Vector3.Zero;
407 m_angularMotorTimescale = 4;
408 m_angularMotorDecayTimescale = 4;
409 m_VhoverHeight = 0;
410// m_VhoverEfficiency = 0.5f;
411 m_VhoverTimescale = 1000;
412 m_VehicleBuoyancy = 0;
413 // m_linearDeflectionEfficiency = 0.5f;
414 // m_linearDeflectionTimescale = 3;
415 // m_angularDeflectionEfficiency = 1;
416 // m_angularDeflectionTimescale = 2;
417 m_verticalAttractionEfficiency = 0.9f;
418 m_verticalAttractionTimescale = 2f;
419 // m_bankingEfficiency = 1;
420 // m_bankingMix = 0.7f;
421 // m_bankingTimescale = 2;
422 // m_referenceFrame = Quaternion.Identity;
423 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
424 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
425 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
426 break;
427 case Vehicle.TYPE_BALLOON:
428 m_linearFrictionTimescale = new Vector3(5, 5, 5);
429 m_angularFrictionTimescale = new Vector3(10, 10, 10);
430 m_linearMotorTimescale = 5;
431 m_linearMotorDecayTimescale = 60;
432 m_angularMotorDirection = Vector3.Zero;
433 m_angularMotorDVel = Vector3.Zero;
434 m_angularMotorTimescale = 6;
435 m_angularMotorDecayTimescale = 10;
436 m_VhoverHeight = 5;
437// m_VhoverEfficiency = 0.8f;
438 m_VhoverTimescale = 10;
439 m_VehicleBuoyancy = 1;
440 // m_linearDeflectionEfficiency = 0;
441 // m_linearDeflectionTimescale = 5;
442 // m_angularDeflectionEfficiency = 0;
443 // m_angularDeflectionTimescale = 5;
444 m_verticalAttractionEfficiency = 1f;
445 m_verticalAttractionTimescale = 100f;
446 // m_bankingEfficiency = 0;
447 // m_bankingMix = 0.7f;
448 // m_bankingTimescale = 5;
449 // m_referenceFrame = Quaternion.Identity;
450 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
451 VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
452 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
453 break;
454
455 }
456 }//end SetDefaultsForType
457
458 internal void Enable(IntPtr pBody, OdeScene pParentScene)
459 {
460 if (m_type == Vehicle.TYPE_NONE)
461 return;
462
463 m_body = pBody;
464 }
465
466 internal void Step(float pTimestep, OdeScene pParentScene)
467 {
468 if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
469 return;
470 frcount++; // used to limit debug comment output
471 if (frcount > 24)
472 frcount = 0;
473
474 MoveLinear(pTimestep, pParentScene);
475 MoveAngular(pTimestep);
476 }// end Step
477
478 internal void Halt()
479 { // Kill all motions, when non-physical
480 m_linearMotorDirection = Vector3.Zero;
481 m_lLinMotorDVel = Vector3.Zero;
482 m_lLinObjectVel = Vector3.Zero;
483 m_wLinObjectVel = Vector3.Zero;
484 m_angularMotorDirection = Vector3.Zero;
485 m_lastAngularVelocity = Vector3.Zero;
486 m_angularMotorDVel = Vector3.Zero;
487 }
488
489 private void UpdateLinDecay()
490 {
491 if (Math.Abs(m_linearMotorDirection.X) > Math.Abs(m_lLinMotorDVel.X)) m_lLinMotorDVel.X = m_linearMotorDirection.X;
492 if (Math.Abs(m_linearMotorDirection.Y) > Math.Abs(m_lLinMotorDVel.Y)) m_lLinMotorDVel.Y = m_linearMotorDirection.Y;
493 if (Math.Abs(m_linearMotorDirection.Z) > Math.Abs(m_lLinMotorDVel.Z)) m_lLinMotorDVel.Z = m_linearMotorDirection.Z;
494 } // else let the motor decay on its own
495
496 private void MoveLinear(float pTimestep, OdeScene _pParentScene)
497 {
498 Vector3 acceleration = new Vector3(0f, 0f, 0f);
499
500 d.Quaternion rot = d.BodyGetQuaternion(Body);
501 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
502 Quaternion irotq = Quaternion.Inverse(rotq);
503 d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame
504 Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z);
505 acceleration = vel_now - m_wLinObjectVel;
506 m_lLinObjectVel = vel_now * irotq;
507
508 if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate
509 {
510 if ( Vector3.Mag(m_lLinMotorDVel) < 1.0f)
511 {
512 float decayfactor = m_linearMotorDecayTimescale/pTimestep;
513 Vector3 decayAmount = (m_lLinMotorDVel/decayfactor);
514 m_lLinMotorDVel -= decayAmount;
515 }
516 else
517 {
518 float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale)));
519 Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * pTimestep;
520 m_lLinMotorDVel -= decel;
521 }
522 if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
523 {
524 m_lLinMotorDVel = Vector3.Zero;
525 }
526 else
527 {
528 if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X;
529 if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y;
530 if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z;
531 }
532 }
533
534 if ( (! m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) )
535 {
536 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
537 if (m_linearMotorTimescale < 300.0f)
538 {
539 Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel;
540 float linfactor = m_linearMotorTimescale/pTimestep;
541 Vector3 attackAmount = (attack_error/linfactor) * 1.3f;
542 m_lLinObjectVel += attackAmount;
543 }
544 if (m_linearFrictionTimescale.X < 300.0f)
545 {
546 float fricfactor = m_linearFrictionTimescale.X / pTimestep;
547 float fricX = m_lLinObjectVel.X / fricfactor;
548 m_lLinObjectVel.X -= fricX;
549 }
550 if (m_linearFrictionTimescale.Y < 300.0f)
551 {
552 float fricfactor = m_linearFrictionTimescale.Y / pTimestep;
553 float fricY = m_lLinObjectVel.Y / fricfactor;
554 m_lLinObjectVel.Y -= fricY;
555 }
556 if (m_linearFrictionTimescale.Z < 300.0f)
557 {
558 float fricfactor = m_linearFrictionTimescale.Z / pTimestep;
559//if(frcount == 0) Console.WriteLine("Zfric={0}", fricfactor);
560 float fricZ = m_lLinObjectVel.Z / fricfactor;
561 m_lLinObjectVel.Z -= fricZ;
562 }
563 }
564 m_wLinObjectVel = m_lLinObjectVel * rotq;
565 // Add Gravity and Buoyancy
566 Vector3 grav = Vector3.Zero;
567 if(m_VehicleBuoyancy < 1.0f)
568 {
569 // There is some gravity, make a gravity force vector
570 // that is applied after object velocity.
571 d.Mass objMass;
572 d.BodyGetMass(Body, out objMass);
573 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
574 grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force
575 } // else its 1.0, no gravity.
576
577 // Check if hovering
578 if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
579 {
580 // We should hover, get the target height
581 d.Vector3 pos = d.BodyGetPosition(Body);
582 if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY)
583 {
584 m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight;
585 }
586 else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY)
587 {
588 m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
589 }
590 else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT)
591 {
592 m_VhoverTargetHeight = m_VhoverHeight;
593 }
594
595 if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY)
596 {
597 // If body is aready heigher, use its height as target height
598 if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
599 }
600
601// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
602// m_VhoverTimescale = 0f; // time to acheive height
603// pTimestep is time since last frame,in secs
604 float herr0 = pos.Z - m_VhoverTargetHeight;
605 // Replace Vertical speed with correction figure if significant
606 if(Math.Abs(herr0) > 0.01f )
607 {
608 d.Mass objMass;
609 d.BodyGetMass(Body, out objMass);
610 m_wLinObjectVel.Z = - ( (herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
611 //KF: m_VhoverEfficiency is not yet implemented
612 }
613 else
614 {
615 m_wLinObjectVel.Z = 0f;
616 }
617 }
618 else
619 { // not hovering, Gravity rules
620 m_wLinObjectVel.Z = vel_now.Z;
621//if(frcount == 0) Console.WriteLine(" Z {0} a.Z {1}", m_wLinObjectVel.Z, acceleration.Z);
622 }
623 // Apply velocity
624 d.BodySetLinearVel(Body, m_wLinObjectVel.X, m_wLinObjectVel.Y, m_wLinObjectVel.Z);
625 // apply gravity force
626 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
627//if(frcount == 0) Console.WriteLine("Grav {0}", grav);
628 } // end MoveLinear()
629
630 private void UpdateAngDecay()
631 {
632 if (Math.Abs(m_angularMotorDirection.X) > Math.Abs(m_angularMotorDVel.X)) m_angularMotorDVel.X = m_angularMotorDirection.X;
633 if (Math.Abs(m_angularMotorDirection.Y) > Math.Abs(m_angularMotorDVel.Y)) m_angularMotorDVel.Y = m_angularMotorDirection.Y;
634 if (Math.Abs(m_angularMotorDirection.Z) > Math.Abs(m_angularMotorDVel.Z)) m_angularMotorDVel.Z = m_angularMotorDirection.Z;
635 } // else let the motor decay on its own
636
637 private void MoveAngular(float pTimestep)
638 {
639 /*
640 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
641
642 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
643 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
644 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
645
646 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
647 private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body
648 */
649//if(frcount == 0) Console.WriteLine("MoveAngular ");
650
651 // Get what the body is doing, this includes 'external' influences
652 d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body);
653 Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z);
654//if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel);
655// Vector3 FrAaccel = m_lastAngularVelocity - angObjectVel;
656// Vector3 initavel = angObjectVel;
657 // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack.
658 float atk_decayfactor = 23.0f / (m_angularMotorTimescale * pTimestep);
659 m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor;
660 // Decay Angular Motor 2.
661 if (m_angularMotorDecayTimescale < 300.0f)
662 {
663//####
664 if ( Vector3.Mag(m_angularMotorDVel) < 1.0f)
665 {
666 float decayfactor = (m_angularMotorDecayTimescale)/pTimestep;
667 Vector3 decayAmount = (m_angularMotorDVel/decayfactor);
668 m_angularMotorDVel -= decayAmount;
669 }
670 else
671 {
672 Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * pTimestep / m_angularMotorDecayTimescale;
673 m_angularMotorDVel -= decel;
674 }
675
676 if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
677 {
678 m_angularMotorDVel = Vector3.Zero;
679 }
680 else
681 {
682 if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X;
683 if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y;
684 if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z;
685 }
686 } // end decay angular motor
687//if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel);
688
689//if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel);
690 // Vertical attractor section
691 Vector3 vertattr = Vector3.Zero;
692
693 if(m_verticalAttractionTimescale < 300)
694 {
695 float VAservo = 1.0f / (m_verticalAttractionTimescale * pTimestep);
696 // get present body rotation
697 d.Quaternion rot = d.BodyGetQuaternion(Body);
698 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
699 // make a vector pointing up
700 Vector3 verterr = Vector3.Zero;
701 verterr.Z = 1.0f;
702 // rotate it to Body Angle
703 verterr = verterr * rotq;
704 // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1.
705 // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go
706 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
707
708 if (verterr.Z < 0.0f)
709 { // Defelction from vertical exceeds 90-degrees. This method will ensure stable return to
710 // vertical, BUT for some reason a z-rotation is imparted to the object. TBI.
711//Console.WriteLine("InvertFlip");
712 verterr.X = 2.0f - verterr.X;
713 verterr.Y = 2.0f - verterr.Y;
714 }
715 verterr *= 0.5f;
716 // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt)
717
718 if ((!angObjectVel.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f))
719 {
720//if(frcount == 0)
721 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
722 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
723 vertattr.X = verterr.Y;
724 vertattr.Y = - verterr.X;
725 vertattr.Z = 0f;
726//if(frcount == 0) Console.WriteLine("VAerr=" + verterr);
727
728 // scaling appears better usingsquare-law
729 float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
730 float bounce = 1.0f - damped;
731 // 0 = crit damp, 1 = bouncy
732 float oavz = angObjectVel.Z; // retain z velocity
733 angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce; // The time-scaled correction, which sums, therefore is bouncy
734 angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped); // damped, good @ < 90.
735 angObjectVel.Z = oavz;
736//if(frcount == 0) Console.WriteLine("VA+");
737//Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel);
738 }
739 else
740 {
741 // else error is very small
742 angObjectVel.X = 0f;
743 angObjectVel.Y = 0f;
744//if(frcount == 0) Console.WriteLine("VA0");
745 }
746 } // else vertical attractor is off
747//if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel);
748
749 if ( (! m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) )
750 { // if motor or object have motion
751 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
752
753 if (m_angularMotorTimescale < 300.0f)
754 {
755 Vector3 attack_error = m_angularMotorDVel - angObjectVel;
756 float angfactor = m_angularMotorTimescale/pTimestep;
757 Vector3 attackAmount = (attack_error/angfactor);
758 angObjectVel += attackAmount;
759//if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount);
760//if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel);
761 }
762
763 angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / pTimestep);
764 angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / pTimestep);
765 angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / pTimestep);
766 } // else no signif. motion
767
768//if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel);
769 // Bank section tba
770 // Deflection section tba
771//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel);
772
773 m_lastAngularVelocity = angObjectVel;
774/*
775 if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.0001f))
776 {
777 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
778 }
779 else
780 {
781 m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
782 }
783 */
784 // Apply to the body
785// Vector3 aInc = m_lastAngularVelocity - initavel;
786//if(frcount == 0) Console.WriteLine("Inc {0}", aInc);
787 d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
788//if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity);
789
790 } //end MoveAngular
791 }
792}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
index 0eb9bb5..5e6696e 100644
--- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
@@ -22,32 +22,10 @@
22 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 22 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
23 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 23 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
24 * 24 *
25 * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces 25 * Revised March 5th 2010 by Kitto Flora. ODEDynamics.cs
26 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: 26 * rolled into ODEPrim.cs
27 * ODEPrim.cs contains methods dealing with Prim editing, Prim
28 * characteristics and Kinetic motion.
29 * ODEDynamics.cs contains methods dealing with Prim Physical motion
30 * (dynamics) and the associated settings. Old Linear and angular
31 * motors for dynamic motion have been replace with MoveLinear()
32 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
33 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
34 * switch between 'VEHICLE' parameter use and general dynamics
35 * settings use.
36 */ 27 */
37 28
38/*
39 * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces
40 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
41 * ODEPrim.cs contains methods dealing with Prim editing, Prim
42 * characteristics and Kinetic motion.
43 * ODEDynamics.cs contains methods dealing with Prim Physical motion
44 * (dynamics) and the associated settings. Old Linear and angular
45 * motors for dynamic motion have been replace with MoveLinear()
46 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
47 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
48 * switch between 'VEHICLE' parameter use and general dynamics
49 * settings use.
50 */
51using System; 29using System;
52using System.Collections.Generic; 30using System.Collections.Generic;
53using System.Reflection; 31using System.Reflection;
@@ -59,6 +37,7 @@ using Ode.NET;
59using OpenSim.Framework; 37using OpenSim.Framework;
60using OpenSim.Region.Physics.Manager; 38using OpenSim.Region.Physics.Manager;
61 39
40
62namespace OpenSim.Region.Physics.OdePlugin 41namespace OpenSim.Region.Physics.OdePlugin
63{ 42{
64 /// <summary> 43 /// <summary>
@@ -85,8 +64,10 @@ namespace OpenSim.Region.Physics.OdePlugin
85 private Vector3 m_taintVelocity; 64 private Vector3 m_taintVelocity;
86 private Vector3 m_taintTorque; 65 private Vector3 m_taintTorque;
87 private Quaternion m_taintrot; 66 private Quaternion m_taintrot;
88 private Vector3 m_angularlock = Vector3.One; 67 private Vector3 m_angularEnable = Vector3.One; // Current setting
89 private Vector3 m_taintAngularLock = Vector3.One; 68 private Vector3 m_taintAngularLock = Vector3.One; // Request from LSL
69
70
90 private IntPtr Amotor = IntPtr.Zero; 71 private IntPtr Amotor = IntPtr.Zero;
91 72
92 private Vector3 m_PIDTarget; 73 private Vector3 m_PIDTarget;
@@ -100,8 +81,8 @@ namespace OpenSim.Region.Physics.OdePlugin
100 private float m_APIDDamping = 0.5f; 81 private float m_APIDDamping = 0.5f;
101 private bool m_useAPID = false; 82 private bool m_useAPID = false;
102 83
103 // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), 84 // These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
104 // and are for non-VEHICLES only. 85 // do not confuse with VEHICLE HOVER
105 86
106 private float m_PIDHoverHeight; 87 private float m_PIDHoverHeight;
107 private float m_PIDHoverTau; 88 private float m_PIDHoverTau;
@@ -110,7 +91,7 @@ namespace OpenSim.Region.Physics.OdePlugin
110 private float m_targetHoverHeight; 91 private float m_targetHoverHeight;
111 private float m_groundHeight; 92 private float m_groundHeight;
112 private float m_waterHeight; 93 private float m_waterHeight;
113 private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. 94 private float m_buoyancy; //m_buoyancy set by llSetBuoyancy()
114 95
115 // private float m_tensor = 5f; 96 // private float m_tensor = 5f;
116 private int body_autodisable_frames = 20; 97 private int body_autodisable_frames = 20;
@@ -181,7 +162,7 @@ namespace OpenSim.Region.Physics.OdePlugin
181 public bool outofBounds; 162 public bool outofBounds;
182 private float m_density = 10.000006836f; // Aluminum g/cm3; 163 private float m_density = 10.000006836f; // Aluminum g/cm3;
183 164
184 public bool _zeroFlag; 165 public bool _zeroFlag; // if body has been stopped
185 private bool m_lastUpdateSent; 166 private bool m_lastUpdateSent;
186 167
187 public IntPtr Body = IntPtr.Zero; 168 public IntPtr Body = IntPtr.Zero;
@@ -196,18 +177,79 @@ namespace OpenSim.Region.Physics.OdePlugin
196 177
197 public volatile bool childPrim; 178 public volatile bool childPrim;
198 179
199 private ODEDynamics m_vehicle;
200
201 internal int m_material = (int)Material.Wood; 180 internal int m_material = (int)Material.Wood;
202 181
203 private int frcount = 0; // Used to limit dynamics debug output to 182 private int frcount = 0; // Used to limit dynamics debug output to
204 183
184 private IntPtr m_body = IntPtr.Zero;
185
186 // Vehicle properties ============================================================================================
187 private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
188 // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
189 private VehicleFlag m_flags = (VehicleFlag) 0; // Bit settings:
190 // HOVER_TERRAIN_ONLY
191 // HOVER_GLOBAL_HEIGHT
192 // NO_DEFLECTION_UP
193 // HOVER_WATER_ONLY
194 // HOVER_UP_ONLY
195 // LIMIT_MOTOR_UP
196 // LIMIT_ROLL_ONLY
197
198 // Linear properties
199 private Vector3 m_linearMotorDirection = Vector3.Zero; // (was m_linearMotorDirectionLASTSET) the (local) Velocity
200 //requested by LSL
201 private float m_linearMotorTimescale = 0; // Motor Attack rate set by LSL
202 private float m_linearMotorDecayTimescale = 0; // Motor Decay rate set by LSL
203 private Vector3 m_linearFrictionTimescale = Vector3.Zero; // General Friction set by LSL
204
205 private Vector3 m_lLinMotorDVel = Vector3.Zero; // decayed motor
206 private Vector3 m_lLinObjectVel = Vector3.Zero; // local frame object velocity
207 private Vector3 m_wLinObjectVel = Vector3.Zero; // world frame object velocity
208
209 //Angular properties
210 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
211
212 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
213 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
214 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
215
216 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
217// private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity
218 private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
219
220 //Deflection properties
221 // private float m_angularDeflectionEfficiency = 0;
222 // private float m_angularDeflectionTimescale = 0;
223 // private float m_linearDeflectionEfficiency = 0;
224 // private float m_linearDeflectionTimescale = 0;
225
226 //Banking properties
227 // private float m_bankingEfficiency = 0;
228 // private float m_bankingMix = 0;
229 // private float m_bankingTimescale = 0;
230
231 //Hover and Buoyancy properties
232 private float m_VhoverHeight = 0f;
233// private float m_VhoverEfficiency = 0f;
234 private float m_VhoverTimescale = 0f;
235 private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height
236 private float m_VehicleBuoyancy = 0f; // Set by VEHICLE_BUOYANCY, for a vehicle.
237 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
238 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
239 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
240
241 //Attractor properties
242 private float m_verticalAttractionEfficiency = 1.0f; // damped
243 private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor.
244
245
246
247
248
205 249
206 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, 250 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
207 Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode) 251 Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode)
208 { 252 {
209 m_vehicle = new ODEDynamics();
210 //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
211 ode = dode; 253 ode = dode;
212 if (!pos.IsFinite()) 254 if (!pos.IsFinite())
213 { 255 {
@@ -300,7 +342,7 @@ namespace OpenSim.Region.Physics.OdePlugin
300 { 342 {
301 set { 343 set {
302 344
303 345//Console.WriteLine("Sel {0} {1} {2}", m_primName, value, m_isphysical);
304 // This only makes the object not collidable if the object 346 // This only makes the object not collidable if the object
305 // is physical or the object is modified somehow *IN THE FUTURE* 347 // is physical or the object is modified somehow *IN THE FUTURE*
306 // without this, if an avatar selects prim, they can walk right 348 // without this, if an avatar selects prim, they can walk right
@@ -320,6 +362,403 @@ namespace OpenSim.Region.Physics.OdePlugin
320 } 362 }
321 } 363 }
322 364
365 public override bool IsPhysical
366 {
367 get { return m_isphysical; }
368 set
369 {
370 m_isphysical = value;
371 if (!m_isphysical)
372 { // Zero the remembered last velocity
373 m_lastVelocity = Vector3.Zero;
374 if (m_type != Vehicle.TYPE_NONE) Halt();
375 }
376 }
377 }
378
379 public void setPrimForRemoval()
380 {
381 m_taintremove = true;
382 }
383
384 public override bool Flying
385 {
386 // no flying prims for you
387 get { return false; }
388 set { }
389 }
390
391 public override bool IsColliding
392 {
393 get { return iscolliding; }
394 set { iscolliding = value; }
395 }
396
397 public override bool CollidingGround
398 {
399 get { return false; }
400 set { return; }
401 }
402
403 public override bool CollidingObj
404 {
405 get { return false; }
406 set { return; }
407 }
408
409 public override bool ThrottleUpdates
410 {
411 get { return m_throttleUpdates; }
412 set { m_throttleUpdates = value; }
413 }
414
415 public override bool Stopped
416 {
417 get { return _zeroFlag; }
418 }
419
420 public override Vector3 Position
421 {
422 get { return _position; }
423
424 set { _position = value;
425 //m_log.Info("[PHYSICS]: " + _position.ToString());
426 }
427 }
428
429 public override Vector3 Size
430 {
431 get { return _size; }
432 set
433 {
434 if (value.IsFinite())
435 {
436 _size = value;
437 }
438 else
439 {
440 m_log.Warn("[PHYSICS]: Got NaN Size on object");
441 }
442 }
443 }
444
445 public override float Mass
446 {
447 get { return CalculateMass(); }
448 }
449
450 public override Vector3 Force
451 {
452 //get { return Vector3.Zero; }
453 get { return m_force; }
454 set
455 {
456 if (value.IsFinite())
457 {
458 m_force = value;
459 }
460 else
461 {
462 m_log.Warn("[PHYSICS]: NaN in Force Applied to an Object");
463 }
464 }
465 }
466
467 public override int VehicleType
468 {
469 get { return (int)m_type; }
470 set { ProcessTypeChange((Vehicle)value); }
471 }
472
473 public override void VehicleFloatParam(int param, float value)
474 {
475 ProcessFloatVehicleParam((Vehicle) param, value);
476 }
477
478 public override void VehicleVectorParam(int param, Vector3 value)
479 {
480 ProcessVectorVehicleParam((Vehicle) param, value);
481 }
482
483 public override void VehicleRotationParam(int param, Quaternion rotation)
484 {
485 ProcessRotationVehicleParam((Vehicle) param, rotation);
486 }
487
488 public override void VehicleFlags(int param, bool remove)
489 {
490 ProcessVehicleFlags(param, remove);
491 }
492
493 public override void SetVolumeDetect(int param)
494 {
495 lock (_parent_scene.OdeLock)
496 {
497 m_isVolumeDetect = (param!=0);
498 }
499 }
500
501 public override Vector3 CenterOfMass
502 {
503 get { return Vector3.Zero; }
504 }
505
506 public override Vector3 GeometricCenter
507 {
508 get { return Vector3.Zero; }
509 }
510
511 public override PrimitiveBaseShape Shape
512 {
513 set
514 {
515 _pbs = value;
516 m_taintshape = true;
517 }
518 }
519
520 public override Vector3 Velocity
521 {
522 get
523 {
524 // Averate previous velocity with the new one so
525 // client object interpolation works a 'little' better
526 if (_zeroFlag)
527 return Vector3.Zero;
528
529 Vector3 returnVelocity = Vector3.Zero;
530 returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2;
531 returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2;
532 returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2;
533 return returnVelocity;
534 }
535 set
536 {
537 if (value.IsFinite())
538 {
539 _velocity = value;
540
541 m_taintVelocity = value;
542 _parent_scene.AddPhysicsActorTaint(this);
543 }
544 else
545 {
546 m_log.Warn("[PHYSICS]: Got NaN Velocity in Object");
547 }
548
549 }
550 }
551
552 public override Vector3 Torque
553 {
554 get
555 {
556 if (!m_isphysical || Body == IntPtr.Zero)
557 return Vector3.Zero;
558
559 return _torque;
560 }
561
562 set
563 {
564 if (value.IsFinite())
565 {
566 m_taintTorque = value;
567 _parent_scene.AddPhysicsActorTaint(this);
568 }
569 else
570 {
571 m_log.Warn("[PHYSICS]: Got NaN Torque in Object");
572 }
573 }
574 }
575
576 public override float CollisionScore
577 {
578 get { return m_collisionscore; }
579 set { m_collisionscore = value; }
580 }
581
582 public override bool Kinematic
583 {
584 get { return false; }
585 set { }
586 }
587
588 public override Quaternion Orientation
589 {
590 get { return _orientation; }
591 set
592 {
593 if (QuaternionIsFinite(value))
594 {
595 _orientation = value;
596 }
597 else
598 m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object");
599
600 }
601 }
602
603
604 public override bool FloatOnWater
605 {
606 set {
607 m_taintCollidesWater = value;
608 _parent_scene.AddPhysicsActorTaint(this);
609 }
610 }
611
612 public override void SetMomentum(Vector3 momentum)
613 {
614 }
615
616 public override Vector3 PIDTarget
617 {
618 set
619 {
620 if (value.IsFinite())
621 {
622 m_PIDTarget = value;
623 }
624 else
625 m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object");
626 }
627 }
628 public override bool PIDActive { set { m_usePID = value; } }
629 public override float PIDTau { set { m_PIDTau = value; } }
630
631 // For RotLookAt
632 public override Quaternion APIDTarget { set { m_APIDTarget = value; } }
633 public override bool APIDActive { set { m_useAPID = value; } }
634 public override float APIDStrength { set { m_APIDStrength = value; } }
635 public override float APIDDamping { set { m_APIDDamping = value; } }
636
637 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
638 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
639 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
640 public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
641
642 internal static bool QuaternionIsFinite(Quaternion q)
643 {
644 if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
645 return false;
646 if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
647 return false;
648 if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
649 return false;
650 if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
651 return false;
652 return true;
653 }
654
655 public override Vector3 Acceleration // client updates read data via here
656 {
657 get { return _acceleration; }
658 }
659
660
661 public void SetAcceleration(Vector3 accel) // No one calls this, and it would not do anything.
662 {
663 _acceleration = accel;
664 }
665
666 public override void AddForce(Vector3 force, bool pushforce)
667 {
668 if (force.IsFinite())
669 {
670 lock (m_forcelist)
671 m_forcelist.Add(force);
672
673 m_taintforce = true;
674 }
675 else
676 {
677 m_log.Warn("[PHYSICS]: Got Invalid linear force vector from Scene in Object");
678 }
679 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
680 }
681
682 public override void AddAngularForce(Vector3 force, bool pushforce)
683 {
684 if (force.IsFinite())
685 {
686 m_angularforcelist.Add(force);
687 m_taintaddangularforce = true;
688 }
689 else
690 {
691 m_log.Warn("[PHYSICS]: Got Invalid Angular force vector from Scene in Object");
692 }
693 }
694
695 public override Vector3 RotationalVelocity
696 {
697 get
698 {
699 return m_rotationalVelocity;
700 }
701 set
702 {
703 if (value.IsFinite())
704 {
705 m_rotationalVelocity = value;
706 }
707 else
708 {
709 m_log.Warn("[PHYSICS]: Got NaN RotationalVelocity in Object");
710 }
711 }
712 }
713
714 public override void CrossingFailure()
715 {
716 m_crossingfailures++;
717 if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
718 {
719 base.RaiseOutOfBounds(_position);
720 return;
721 }
722 else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
723 {
724 m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName);
725 }
726 }
727
728 public override float Buoyancy
729 {
730 get { return m_buoyancy; }
731 set { m_buoyancy = value; }
732 }
733
734 public override void link(PhysicsActor obj)
735 {
736 m_taintparent = obj;
737 }
738
739 public override void delink()
740 {
741 m_taintparent = null;
742 }
743
744 public override void LockAngularMotion(Vector3 axis)
745 {
746 // reverse the zero/non zero values for ODE.
747 if (axis.IsFinite())
748 {
749 axis.X = (axis.X > 0) ? 1f : 0f;
750 axis.Y = (axis.Y > 0) ? 1f : 0f;
751 axis.Z = (axis.Z > 0) ? 1f : 0f;
752 m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
753 m_taintAngularLock = axis;
754 }
755 else
756 {
757 m_log.Warn("[PHYSICS]: Got NaN locking axis from Scene on Object");
758 }
759 }
760
761
323 public void SetGeom(IntPtr geom) 762 public void SetGeom(IntPtr geom)
324 { 763 {
325 prev_geom = prim_geom; 764 prev_geom = prim_geom;
@@ -343,8 +782,6 @@ namespace OpenSim.Region.Physics.OdePlugin
343 //m_log.Warn("Setting Geom to: " + prim_geom); 782 //m_log.Warn("Setting Geom to: " + prim_geom);
344 } 783 }
345 784
346
347
348 public void enableBodySoft() 785 public void enableBodySoft()
349 { 786 {
350 if (!childPrim) 787 if (!childPrim)
@@ -352,8 +789,8 @@ namespace OpenSim.Region.Physics.OdePlugin
352 if (m_isphysical && Body != IntPtr.Zero) 789 if (m_isphysical && Body != IntPtr.Zero)
353 { 790 {
354 d.BodyEnable(Body); 791 d.BodyEnable(Body);
355 if (m_vehicle.Type != Vehicle.TYPE_NONE) 792 if (m_type != Vehicle.TYPE_NONE)
356 m_vehicle.Enable(Body, _parent_scene); 793 Enable(Body, _parent_scene);
357 } 794 }
358 795
359 m_disabled = false; 796 m_disabled = false;
@@ -404,14 +841,9 @@ namespace OpenSim.Region.Physics.OdePlugin
404 m_collisionscore = 0; 841 m_collisionscore = 0;
405 m_disabled = false; 842 m_disabled = false;
406 843
407 // The body doesn't already have a finite rotation mode set here 844 if (m_type != Vehicle.TYPE_NONE)
408 if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0.0f)) && _parent == null)
409 { 845 {
410 createAMotor(m_angularlock); 846 Enable(Body, _parent_scene);
411 }
412 if (m_vehicle.Type != Vehicle.TYPE_NONE)
413 {
414 m_vehicle.Enable(Body, _parent_scene);
415 } 847 }
416 848
417 _parent_scene.addActivePrim(this); 849 _parent_scene.addActivePrim(this);
@@ -892,9 +1324,8 @@ namespace OpenSim.Region.Physics.OdePlugin
892 // } 1324 // }
893 } 1325 }
894 1326
895 public void ProcessTaints(float timestep) 1327 public void ProcessTaints(float timestep) //=============================================================================
896 { 1328 {
897//Console.WriteLine("ProcessTaints for " + m_primName );
898 if (m_taintadd) 1329 if (m_taintadd)
899 { 1330 {
900 changeadd(timestep); 1331 changeadd(timestep);
@@ -958,8 +1389,8 @@ namespace OpenSim.Region.Physics.OdePlugin
958 1389
959 if (m_taintCollidesWater != m_collidesWater) 1390 if (m_taintCollidesWater != m_collidesWater)
960 changefloatonwater(timestep); 1391 changefloatonwater(timestep);
961 1392
962 if (!m_angularlock.ApproxEquals(m_taintAngularLock,0f)) 1393 if (!m_angularEnable.ApproxEquals(m_taintAngularLock,0f))
963 changeAngularLock(timestep); 1394 changeAngularLock(timestep);
964 1395
965 } 1396 }
@@ -970,34 +1401,12 @@ namespace OpenSim.Region.Physics.OdePlugin
970 } 1401 }
971 1402
972 1403
973 private void changeAngularLock(float timestep) 1404 private void changeAngularLock(float timestep)
974 { 1405 {
975 // do we have a Physical object? 1406 if (_parent == null)
976 if (Body != IntPtr.Zero)
977 { 1407 {
978 //Check that we have a Parent 1408 m_angularEnable = m_taintAngularLock;
979 //If we have a parent then we're not authorative here
980 if (_parent == null)
981 {
982 if (!m_taintAngularLock.ApproxEquals(Vector3.One, 0f))
983 {
984 //d.BodySetFiniteRotationMode(Body, 0);
985 //d.BodySetFiniteRotationAxis(Body,m_taintAngularLock.X,m_taintAngularLock.Y,m_taintAngularLock.Z);
986 createAMotor(m_taintAngularLock);
987 }
988 else
989 {
990 if (Amotor != IntPtr.Zero)
991 {
992 d.JointDestroy(Amotor);
993 Amotor = IntPtr.Zero;
994 }
995 }
996 }
997 } 1409 }
998 // Store this for later in case we get turned into a separate body
999 m_angularlock = m_taintAngularLock;
1000
1001 } 1410 }
1002 1411
1003 private void changelink(float timestep) 1412 private void changelink(float timestep)
@@ -1010,7 +1419,6 @@ namespace OpenSim.Region.Physics.OdePlugin
1010 { 1419 {
1011 OdePrim obj = (OdePrim)m_taintparent; 1420 OdePrim obj = (OdePrim)m_taintparent;
1012 //obj.disableBody(); 1421 //obj.disableBody();
1013//Console.WriteLine("changelink calls ParentPrim");
1014 obj.ParentPrim(this); 1422 obj.ParentPrim(this);
1015 1423
1016 /* 1424 /*
@@ -1028,8 +1436,6 @@ namespace OpenSim.Region.Physics.OdePlugin
1028 // destroy link 1436 // destroy link
1029 else if (_parent != null && m_taintparent == null) 1437 else if (_parent != null && m_taintparent == null)
1030 { 1438 {
1031//Console.WriteLine(" changelink B");
1032
1033 if (_parent is OdePrim) 1439 if (_parent is OdePrim)
1034 { 1440 {
1035 OdePrim obj = (OdePrim)_parent; 1441 OdePrim obj = (OdePrim)_parent;
@@ -1055,7 +1461,6 @@ namespace OpenSim.Region.Physics.OdePlugin
1055 // prim is the child 1461 // prim is the child
1056 public void ParentPrim(OdePrim prim) 1462 public void ParentPrim(OdePrim prim)
1057 { 1463 {
1058//Console.WriteLine("ParentPrim " + m_primName);
1059 if (this.m_localID != prim.m_localID) 1464 if (this.m_localID != prim.m_localID)
1060 { 1465 {
1061 if (Body == IntPtr.Zero) 1466 if (Body == IntPtr.Zero)
@@ -1069,7 +1474,6 @@ namespace OpenSim.Region.Physics.OdePlugin
1069 { 1474 {
1070 if (!childrenPrim.Contains(prim)) 1475 if (!childrenPrim.Contains(prim))
1071 { 1476 {
1072//Console.WriteLine("childrenPrim.Add " + prim);
1073 childrenPrim.Add(prim); 1477 childrenPrim.Add(prim);
1074 1478
1075 foreach (OdePrim prm in childrenPrim) 1479 foreach (OdePrim prm in childrenPrim)
@@ -1139,11 +1543,6 @@ namespace OpenSim.Region.Physics.OdePlugin
1139 prm.m_collisionscore = 0; 1543 prm.m_collisionscore = 0;
1140 prm.m_disabled = false; 1544 prm.m_disabled = false;
1141 1545
1142 // The body doesn't already have a finite rotation mode set here
1143 if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
1144 {
1145 prm.createAMotor(m_angularlock);
1146 }
1147 prm.Body = Body; 1546 prm.Body = Body;
1148 _parent_scene.addActivePrim(prm); 1547 _parent_scene.addActivePrim(prm);
1149 } 1548 }
@@ -1182,13 +1581,8 @@ namespace OpenSim.Region.Physics.OdePlugin
1182 m_collisionscore = 0; 1581 m_collisionscore = 0;
1183 m_disabled = false; 1582 m_disabled = false;
1184 1583
1185 // The body doesn't already have a finite rotation mode set here
1186 if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
1187 {
1188 createAMotor(m_angularlock);
1189 }
1190 d.BodySetPosition(Body, Position.X, Position.Y, Position.Z); 1584 d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);
1191 if (m_vehicle.Type != Vehicle.TYPE_NONE) m_vehicle.Enable(Body, _parent_scene); 1585 if (m_type != Vehicle.TYPE_NONE) Enable(Body, _parent_scene);
1192 _parent_scene.addActivePrim(this); 1586 _parent_scene.addActivePrim(this);
1193 } 1587 }
1194 } 1588 }
@@ -1225,7 +1619,6 @@ namespace OpenSim.Region.Physics.OdePlugin
1225 { 1619 {
1226 foreach (OdePrim prm in childrenPrim) 1620 foreach (OdePrim prm in childrenPrim)
1227 { 1621 {
1228//Console.WriteLine("ChildSetGeom calls ParentPrim");
1229 ParentPrim(prm); 1622 ParentPrim(prm);
1230 } 1623 }
1231 } 1624 }
@@ -1252,30 +1645,21 @@ namespace OpenSim.Region.Physics.OdePlugin
1252 1645
1253 lock (childrenPrim) 1646 lock (childrenPrim)
1254 { 1647 {
1255 //Console.WriteLine("childrenPrim.Remove " + odePrim);
1256 childrenPrim.Remove(odePrim); 1648 childrenPrim.Remove(odePrim);
1257 } 1649 }
1258
1259
1260
1261 1650
1262 if (Body != IntPtr.Zero) 1651 if (Body != IntPtr.Zero)
1263 { 1652 {
1264 _parent_scene.remActivePrim(this); 1653 _parent_scene.remActivePrim(this);
1265 } 1654 }
1266 1655
1267
1268
1269 lock (childrenPrim) 1656 lock (childrenPrim)
1270 { 1657 {
1271 foreach (OdePrim prm in childrenPrim) 1658 foreach (OdePrim prm in childrenPrim)
1272 { 1659 {
1273//Console.WriteLine("ChildDelink calls ParentPrim");
1274 ParentPrim(prm); 1660 ParentPrim(prm);
1275 } 1661 }
1276 } 1662 }
1277
1278
1279 } 1663 }
1280 1664
1281 private void changeSelectedStatus(float timestep) 1665 private void changeSelectedStatus(float timestep)
@@ -1503,17 +1887,22 @@ namespace OpenSim.Region.Physics.OdePlugin
1503 1887
1504 public void changemove(float timestep) 1888 public void changemove(float timestep)
1505 { 1889 {
1890//Console.WriteLine("changemove for {0}", m_primName );
1891
1506 if (m_isphysical) 1892 if (m_isphysical)
1507 { 1893 {
1508 1894//Console.WriteLine("phys {0} {1} {2}", m_disabled, m_taintremove, childPrim);
1509 if (!m_disabled && !m_taintremove && !childPrim) 1895// if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits!
1896 if (!m_taintremove && !childPrim)
1510 { 1897 {
1898//Console.WriteLine("physOK");
1511 if (Body == IntPtr.Zero) 1899 if (Body == IntPtr.Zero)
1512 enableBody(); 1900 enableBody();
1513 //Prim auto disable after 20 frames, 1901 //Prim auto disable after 20 frames,
1514 //if you move it, re-enable the prim manually. 1902 //if you move it, re-enable the prim manually.
1515 if (_parent != null) 1903 if (_parent != null)
1516 { 1904 {
1905//Console.WriteLine("physChild");
1517 if (m_linkJoint != IntPtr.Zero) 1906 if (m_linkJoint != IntPtr.Zero)
1518 { 1907 {
1519 d.JointDestroy(m_linkJoint); 1908 d.JointDestroy(m_linkJoint);
@@ -1522,6 +1911,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1522 } 1911 }
1523 if (Body != IntPtr.Zero) 1912 if (Body != IntPtr.Zero)
1524 { 1913 {
1914//Console.WriteLine("physNotIPZ");
1525 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); 1915 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
1526 1916
1527 if (_parent != null) 1917 if (_parent != null)
@@ -1537,9 +1927,9 @@ Console.WriteLine(" JointCreateFixed");
1537 } 1927 }
1538 } 1928 }
1539 d.BodyEnable(Body); 1929 d.BodyEnable(Body);
1540 if (m_vehicle.Type != Vehicle.TYPE_NONE) 1930 if (m_type != Vehicle.TYPE_NONE)
1541 { 1931 {
1542 m_vehicle.Enable(Body, _parent_scene); 1932 Enable(Body, _parent_scene);
1543 } 1933 }
1544 } 1934 }
1545 else 1935 else
@@ -1554,6 +1944,7 @@ Console.WriteLine(" JointCreateFixed");
1554 } 1944 }
1555 else 1945 else
1556 { 1946 {
1947//Console.WriteLine("NONphys");
1557 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); 1948 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
1558 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); 1949 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
1559 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1950 _parent_scene.waitForSpaceUnlock(m_targetSpace);
@@ -1577,294 +1968,6 @@ Console.WriteLine(" JointCreateFixed");
1577 m_taintposition = _position; 1968 m_taintposition = _position;
1578 } 1969 }
1579 1970
1580 public void Move(float timestep)
1581 {
1582 float fx = 0;
1583 float fy = 0;
1584 float fz = 0;
1585
1586 frcount++; // used to limit debug comment output
1587 if (frcount > 100)
1588 frcount = 0;
1589
1590 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims.
1591 {
1592//if(frcount == 0) Console.WriteLine("Move " + m_primName + " VTyp " + m_vehicle.Type +
1593 // " usePID=" + m_usePID + " seHover=" + m_useHoverPID + " useAPID=" + m_useAPID);
1594 if (m_vehicle.Type != Vehicle.TYPE_NONE)
1595 {
1596 // 'VEHICLES' are dealt with in ODEDynamics.cs
1597 m_vehicle.Step(timestep, _parent_scene);
1598 }
1599 else
1600 {
1601 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
1602 // NON-'VEHICLES' are dealt with here
1603 if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f))
1604 {
1605 d.Vector3 avel2 = d.BodyGetAngularVel(Body);
1606 if (m_angularlock.X == 1)
1607 avel2.X = 0;
1608 if (m_angularlock.Y == 1)
1609 avel2.Y = 0;
1610 if (m_angularlock.Z == 1)
1611 avel2.Z = 0;
1612 d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
1613 }
1614 //float PID_P = 900.0f;
1615
1616 float m_mass = CalculateMass();
1617
1618// fz = 0f;
1619 //m_log.Info(m_collisionFlags.ToString());
1620
1621
1622 //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle.
1623 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
1624 // NB Prims in ODE are no subject to global gravity
1625 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass
1626
1627 if (m_usePID)
1628 {
1629//if(frcount == 0) Console.WriteLine("PID " + m_primName);
1630 // KF - this is for object MoveToTarget.
1631
1632 //if (!d.BodyIsEnabled(Body))
1633 //d.BodySetForce(Body, 0f, 0f, 0f);
1634
1635 // no lock; for now it's only called from within Simulate()
1636
1637 // If the PID Controller isn't active then we set our force
1638 // calculating base velocity to the current position
1639
1640 if ((m_PIDTau < 1) && (m_PIDTau != 0))
1641 {
1642 //PID_G = PID_G / m_PIDTau;
1643 m_PIDTau = 1;
1644 }
1645
1646 if ((PID_G - m_PIDTau) <= 0)
1647 {
1648 PID_G = m_PIDTau + 1;
1649 }
1650 //PidStatus = true;
1651
1652 // PhysicsVector vec = new PhysicsVector();
1653 d.Vector3 vel = d.BodyGetLinearVel(Body);
1654
1655 d.Vector3 pos = d.BodyGetPosition(Body);
1656 _target_velocity =
1657 new Vector3(
1658 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
1659 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
1660 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
1661 );
1662
1663 // if velocity is zero, use position control; otherwise, velocity control
1664
1665 if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
1666 {
1667 // keep track of where we stopped. No more slippin' & slidin'
1668
1669 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1670 // react to the physics scene by moving it's position.
1671 // Avatar to Avatar collisions
1672 // Prim to avatar collisions
1673
1674 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
1675 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
1676 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
1677 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
1678 d.BodySetLinearVel(Body, 0, 0, 0);
1679 d.BodyAddForce(Body, 0, 0, fz);
1680 return;
1681 }
1682 else
1683 {
1684 _zeroFlag = false;
1685
1686 // We're flying and colliding with something
1687 fx = ((_target_velocity.X) - vel.X) * (PID_D);
1688 fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
1689
1690 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
1691
1692 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
1693 }
1694 } // end if (m_usePID)
1695
1696 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
1697 if (m_useHoverPID && !m_usePID)
1698 {
1699//Console.WriteLine("Hover " + m_primName);
1700
1701 // If we're using the PID controller, then we have no gravity
1702 fz = (-1 * _parent_scene.gravityz) * m_mass;
1703
1704 // no lock; for now it's only called from within Simulate()
1705
1706 // If the PID Controller isn't active then we set our force
1707 // calculating base velocity to the current position
1708
1709 if ((m_PIDTau < 1))
1710 {
1711 PID_G = PID_G / m_PIDTau;
1712 }
1713
1714 if ((PID_G - m_PIDTau) <= 0)
1715 {
1716 PID_G = m_PIDTau + 1;
1717 }
1718
1719
1720 // Where are we, and where are we headed?
1721 d.Vector3 pos = d.BodyGetPosition(Body);
1722 d.Vector3 vel = d.BodyGetLinearVel(Body);
1723
1724
1725 // Non-Vehicles have a limited set of Hover options.
1726 // determine what our target height really is based on HoverType
1727 switch (m_PIDHoverType)
1728 {
1729 case PIDHoverType.Ground:
1730 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
1731 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
1732 break;
1733 case PIDHoverType.GroundAndWater:
1734 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
1735 m_waterHeight = _parent_scene.GetWaterLevel();
1736 if (m_groundHeight > m_waterHeight)
1737 {
1738 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
1739 }
1740 else
1741 {
1742 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
1743 }
1744 break;
1745
1746 } // end switch (m_PIDHoverType)
1747
1748
1749 _target_velocity =
1750 new Vector3(0.0f, 0.0f,
1751 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
1752 );
1753
1754 // if velocity is zero, use position control; otherwise, velocity control
1755
1756 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
1757 {
1758 // keep track of where we stopped. No more slippin' & slidin'
1759
1760 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1761 // react to the physics scene by moving it's position.
1762 // Avatar to Avatar collisions
1763 // Prim to avatar collisions
1764
1765 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
1766 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
1767 d.BodyAddForce(Body, 0, 0, fz);
1768 //KF this prevents furthur motions return;
1769 }
1770 else
1771 {
1772 _zeroFlag = false;
1773
1774 // We're flying and colliding with something
1775 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
1776 }
1777 } // end m_useHoverPID && !m_usePID
1778
1779 if (m_useAPID)
1780 {
1781 // RotLookAt, apparently overrides all other rotation sources. Inputs:
1782 // Quaternion m_APIDTarget
1783 // float m_APIDStrength // From SL experiments, this is the time to get there
1784 // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly
1785 // Also in SL the mass of the object has no effect on time to get there.
1786 // Factors:
1787//if(frcount == 0) Console.WriteLine("APID ");
1788 // get present body rotation
1789 float limit = 1.0f;
1790 float scaler = 50f; // adjusts damping time
1791 float RLAservo = 0f;
1792
1793 d.Quaternion rot = d.BodyGetQuaternion(Body);
1794 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
1795 Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget;
1796 float diff_angle;
1797 Vector3 diff_axis;
1798 rot_diff.GetAxisAngle(out diff_axis, out diff_angle);
1799 diff_axis.Normalize();
1800 if(diff_angle > 0.01f) // diff_angle is always +ve
1801 {
1802// PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z);
1803 Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z);
1804 rotforce = rotforce * rotq;
1805 if(diff_angle > limit) diff_angle = limit; // cap the rotate rate
1806// RLAservo = timestep / m_APIDStrength * m_mass * scaler;
1807 // rotforce = rotforce * RLAservo * diff_angle ;
1808 // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z);
1809 RLAservo = timestep / m_APIDStrength * scaler;
1810 rotforce = rotforce * RLAservo * diff_angle ;
1811 d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z);
1812//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo);
1813 }
1814//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle);
1815 } // end m_useAPID
1816
1817 fx *= m_mass;
1818 fy *= m_mass;
1819 //fz *= m_mass;
1820
1821 fx += m_force.X;
1822 fy += m_force.Y;
1823 fz += m_force.Z;
1824
1825 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
1826 if (fx != 0 || fy != 0 || fz != 0)
1827 {
1828 //m_taintdisable = true;
1829 //base.RaiseOutOfBounds(Position);
1830 //d.BodySetLinearVel(Body, fx, fy, 0f);
1831 if (!d.BodyIsEnabled(Body))
1832 {
1833 // A physical body at rest on a surface will auto-disable after a while,
1834 // this appears to re-enable it incase the surface it is upon vanishes,
1835 // and the body should fall again.
1836 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1837 d.BodySetForce(Body, 0, 0, 0);
1838 enableBodySoft();
1839 }
1840
1841 // 35x10 = 350n times the mass per second applied maximum.
1842 float nmax = 35f * m_mass;
1843 float nmin = -35f * m_mass;
1844
1845
1846 if (fx > nmax)
1847 fx = nmax;
1848 if (fx < nmin)
1849 fx = nmin;
1850 if (fy > nmax)
1851 fy = nmax;
1852 if (fy < nmin)
1853 fy = nmin;
1854 d.BodyAddForce(Body, fx, fy, fz);
1855//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
1856 }
1857 }
1858 }
1859 else
1860 { // is not physical, or is not a body or is selected
1861 // _zeroPosition = d.BodyGetPosition(Body);
1862 return;
1863//Console.WriteLine("Nothing " + m_primName);
1864
1865 }
1866 }
1867
1868 1971
1869 1972
1870 public void rotate(float timestep) 1973 public void rotate(float timestep)
@@ -1878,11 +1981,6 @@ Console.WriteLine(" JointCreateFixed");
1878 { 1981 {
1879 // KF: If this is a root prim do BodySet 1982 // KF: If this is a root prim do BodySet
1880 d.BodySetQuaternion(Body, ref myrot); 1983 d.BodySetQuaternion(Body, ref myrot);
1881 if (m_isphysical)
1882 {
1883 if (!m_angularlock.ApproxEquals(Vector3.One, 0f))
1884 createAMotor(m_angularlock);
1885 }
1886 } 1984 }
1887 else 1985 else
1888 { 1986 {
@@ -2293,757 +2391,11 @@ Console.WriteLine(" JointCreateFixed");
2293 m_taintVelocity = Vector3.Zero; 2391 m_taintVelocity = Vector3.Zero;
2294 } 2392 }
2295 2393
2296 public override bool IsPhysical
2297 {
2298 get { return m_isphysical; }
2299 set
2300 {
2301 m_isphysical = value;
2302 if (!m_isphysical)
2303 { // Zero the remembered last velocity
2304 m_lastVelocity = Vector3.Zero;
2305 if (m_vehicle.Type != Vehicle.TYPE_NONE) m_vehicle.Halt();
2306 }
2307 }
2308 }
2309
2310 public void setPrimForRemoval()
2311 {
2312 m_taintremove = true;
2313 }
2314
2315 public override bool Flying
2316 {
2317 // no flying prims for you
2318 get { return false; }
2319 set { }
2320 }
2321
2322 public override bool IsColliding
2323 {
2324 get { return iscolliding; }
2325 set { iscolliding = value; }
2326 }
2327
2328 public override bool CollidingGround
2329 {
2330 get { return false; }
2331 set { return; }
2332 }
2333
2334 public override bool CollidingObj
2335 {
2336 get { return false; }
2337 set { return; }
2338 }
2339
2340 public override bool ThrottleUpdates
2341 {
2342 get { return m_throttleUpdates; }
2343 set { m_throttleUpdates = value; }
2344 }
2345
2346 public override bool Stopped
2347 {
2348 get { return _zeroFlag; }
2349 }
2350
2351 public override Vector3 Position
2352 {
2353 get { return _position; }
2354
2355 set { _position = value;
2356 //m_log.Info("[PHYSICS]: " + _position.ToString());
2357 }
2358 }
2359
2360 public override Vector3 Size
2361 {
2362 get { return _size; }
2363 set
2364 {
2365 if (value.IsFinite())
2366 {
2367 _size = value;
2368 }
2369 else
2370 {
2371 m_log.Warn("[PHYSICS]: Got NaN Size on object");
2372 }
2373 }
2374 }
2375
2376 public override float Mass
2377 {
2378 get { return CalculateMass(); }
2379 }
2380
2381 public override Vector3 Force
2382 {
2383 //get { return Vector3.Zero; }
2384 get { return m_force; }
2385 set
2386 {
2387 if (value.IsFinite())
2388 {
2389 m_force = value;
2390 }
2391 else
2392 {
2393 m_log.Warn("[PHYSICS]: NaN in Force Applied to an Object");
2394 }
2395 }
2396 }
2397
2398 public override int VehicleType
2399 {
2400 get { return (int)m_vehicle.Type; }
2401 set { m_vehicle.ProcessTypeChange((Vehicle)value); }
2402 }
2403
2404 public override void VehicleFloatParam(int param, float value)
2405 {
2406 m_vehicle.ProcessFloatVehicleParam((Vehicle) param, value);
2407 }
2408
2409 public override void VehicleVectorParam(int param, Vector3 value)
2410 {
2411 m_vehicle.ProcessVectorVehicleParam((Vehicle) param, value);
2412 }
2413
2414 public override void VehicleRotationParam(int param, Quaternion rotation)
2415 {
2416 m_vehicle.ProcessRotationVehicleParam((Vehicle) param, rotation);
2417 }
2418
2419 public override void VehicleFlagsSet(int flags)
2420 {
2421 m_vehicle.ProcessFlagsVehicleSet(flags);
2422 }
2423
2424 public override void VehicleFlagsRemove(int flags)
2425 {
2426 m_vehicle.ProcessFlagsVehicleRemove(flags);
2427 }
2428
2429 public override void VehicleFlags(int flags, bool remove)
2430 {
2431 if (!remove)
2432 m_vehicle.ProcessFlagsVehicleSet(flags);
2433 else
2434 m_vehicle.ProcessFlagsVehicleRemove(flags);
2435 }
2436
2437 public override void SetVolumeDetect(int param)
2438 {
2439 lock (_parent_scene.OdeLock)
2440 {
2441 m_isVolumeDetect = (param!=0);
2442 }
2443 }
2444
2445 public override Vector3 CenterOfMass
2446 {
2447 get { return Vector3.Zero; }
2448 }
2449
2450 public override Vector3 GeometricCenter
2451 {
2452 get { return Vector3.Zero; }
2453 }
2454
2455 public override PrimitiveBaseShape Shape
2456 {
2457 set
2458 {
2459 _pbs = value;
2460 m_taintshape = true;
2461 }
2462 }
2463
2464 public override Vector3 Velocity
2465 {
2466 get
2467 {
2468 // Averate previous velocity with the new one so
2469 // client object interpolation works a 'little' better
2470 if (_zeroFlag)
2471 return Vector3.Zero;
2472
2473 Vector3 returnVelocity = Vector3.Zero;
2474 returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2;
2475 returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2;
2476 returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2;
2477 return returnVelocity;
2478 }
2479 set
2480 {
2481 if (value.IsFinite())
2482 {
2483 _velocity = value;
2484
2485 m_taintVelocity = value;
2486 _parent_scene.AddPhysicsActorTaint(this);
2487 }
2488 else
2489 {
2490 m_log.Warn("[PHYSICS]: Got NaN Velocity in Object");
2491 }
2492
2493 }
2494 }
2495
2496 public override Vector3 Torque
2497 {
2498 get
2499 {
2500 if (!m_isphysical || Body == IntPtr.Zero)
2501 return Vector3.Zero;
2502
2503 return _torque;
2504 }
2505
2506 set
2507 {
2508 if (value.IsFinite())
2509 {
2510 m_taintTorque = value;
2511 _parent_scene.AddPhysicsActorTaint(this);
2512 }
2513 else
2514 {
2515 m_log.Warn("[PHYSICS]: Got NaN Torque in Object");
2516 }
2517 }
2518 }
2519
2520 public override float CollisionScore
2521 {
2522 get { return m_collisionscore; }
2523 set { m_collisionscore = value; }
2524 }
2525
2526 public override bool Kinematic
2527 {
2528 get { return false; }
2529 set { }
2530 }
2531
2532 public override Quaternion Orientation
2533 {
2534 get { return _orientation; }
2535 set
2536 {
2537 if (QuaternionIsFinite(value))
2538 {
2539 _orientation = value;
2540 }
2541 else
2542 m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object");
2543
2544 }
2545 }
2546
2547 internal static bool QuaternionIsFinite(Quaternion q)
2548 {
2549 if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
2550 return false;
2551 if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
2552 return false;
2553 if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
2554 return false;
2555 if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
2556 return false;
2557 return true;
2558 }
2559
2560 public override Vector3 Acceleration
2561 {
2562 get { return _acceleration; }
2563 }
2564
2565
2566 public void SetAcceleration(Vector3 accel)
2567 {
2568 _acceleration = accel;
2569 }
2570
2571 public override void AddForce(Vector3 force, bool pushforce)
2572 {
2573 if (force.IsFinite())
2574 {
2575 lock (m_forcelist)
2576 m_forcelist.Add(force);
2577
2578 m_taintforce = true;
2579 }
2580 else
2581 {
2582 m_log.Warn("[PHYSICS]: Got Invalid linear force vector from Scene in Object");
2583 }
2584 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
2585 }
2586
2587 public override void AddAngularForce(Vector3 force, bool pushforce)
2588 {
2589 if (force.IsFinite())
2590 {
2591 m_angularforcelist.Add(force);
2592 m_taintaddangularforce = true;
2593 }
2594 else
2595 {
2596 m_log.Warn("[PHYSICS]: Got Invalid Angular force vector from Scene in Object");
2597 }
2598 }
2599
2600 public override Vector3 RotationalVelocity
2601 {
2602 get
2603 {
2604/* Vector3 pv = Vector3.Zero;
2605 if (_zeroFlag)
2606 return pv;
2607 m_lastUpdateSent = false;
2608
2609 if (m_rotationalVelocity.ApproxEquals(pv, 0.2f))
2610 return pv;
2611*/
2612 return m_rotationalVelocity;
2613 }
2614 set
2615 {
2616 if (value.IsFinite())
2617 {
2618 m_rotationalVelocity = value;
2619 }
2620 else
2621 {
2622 m_log.Warn("[PHYSICS]: Got NaN RotationalVelocity in Object");
2623 }
2624 }
2625 }
2626
2627 public override void CrossingFailure()
2628 {
2629 m_crossingfailures++;
2630 if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
2631 {
2632 base.RaiseOutOfBounds(_position);
2633 return;
2634 }
2635 else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
2636 {
2637 m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName);
2638 }
2639 }
2640
2641 public override float Buoyancy
2642 {
2643 get { return m_buoyancy; }
2644 set { m_buoyancy = value; }
2645 }
2646
2647 public override void link(PhysicsActor obj)
2648 {
2649 m_taintparent = obj;
2650 }
2651
2652 public override void delink()
2653 {
2654 m_taintparent = null;
2655 }
2656
2657 public override void LockAngularMotion(Vector3 axis)
2658 {
2659 // reverse the zero/non zero values for ODE.
2660 if (axis.IsFinite())
2661 {
2662 axis.X = (axis.X > 0) ? 1f : 0f;
2663 axis.Y = (axis.Y > 0) ? 1f : 0f;
2664 axis.Z = (axis.Z > 0) ? 1f : 0f;
2665 m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
2666 m_taintAngularLock = axis;
2667 }
2668 else
2669 {
2670 m_log.Warn("[PHYSICS]: Got NaN locking axis from Scene on Object");
2671 }
2672 }
2673
2674 public void UpdatePositionAndVelocity() 2394 public void UpdatePositionAndVelocity()
2675 { 2395 {
2676 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! 2396 return; // moved to the Move() method
2677 if (_parent == null)
2678 {
2679 Vector3 pv = Vector3.Zero;
2680 bool lastZeroFlag = _zeroFlag;
2681 if (Body != (IntPtr)0) // FIXME -> or if it is a joint
2682 {
2683 d.Vector3 vec = d.BodyGetPosition(Body);
2684 d.Quaternion ori = d.BodyGetQuaternion(Body);
2685 d.Vector3 vel = d.BodyGetLinearVel(Body);
2686 d.Vector3 rotvel = d.BodyGetAngularVel(Body);
2687 d.Vector3 torque = d.BodyGetTorque(Body);
2688 _torque = new Vector3(torque.X, torque.Y, torque.Z);
2689 Vector3 l_position = Vector3.Zero;
2690 Quaternion l_orientation = Quaternion.Identity;
2691
2692 // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!)
2693 //if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
2694 //if (vec.Y < 0.0f) { vec.Y = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
2695 //if (vec.X > 255.95f) { vec.X = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
2696 //if (vec.Y > 255.95f) { vec.Y = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
2697
2698 m_lastposition = _position;
2699 m_lastorientation = _orientation;
2700
2701 l_position.X = vec.X;
2702 l_position.Y = vec.Y;
2703 l_position.Z = vec.Z;
2704 l_orientation.X = ori.X;
2705 l_orientation.Y = ori.Y;
2706 l_orientation.Z = ori.Z;
2707 l_orientation.W = ori.W;
2708
2709// if(l_position.Y != m_lastposition.Y){
2710// Console.WriteLine("UP&V {0} {1}", m_primName, l_position);
2711// }
2712
2713 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)
2714 {
2715 //base.RaiseOutOfBounds(l_position);
2716
2717 if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)
2718 {
2719 _position = l_position;
2720 //_parent_scene.remActivePrim(this);
2721 if (_parent == null)
2722 base.RequestPhysicsterseUpdate();
2723 return;
2724 }
2725 else
2726 {
2727 if (_parent == null)
2728 base.RaiseOutOfBounds(l_position);
2729 return;
2730 }
2731 }
2732
2733 if (l_position.Z < 0)
2734 {
2735 // This is so prim that get lost underground don't fall forever and suck up
2736 //
2737 // Sim resources and memory.
2738 // Disables the prim's movement physics....
2739 // It's a hack and will generate a console message if it fails.
2740
2741 //IsPhysical = false;
2742 if (_parent == null)
2743 base.RaiseOutOfBounds(_position);
2744
2745 _acceleration.X = 0;
2746 _acceleration.Y = 0;
2747 _acceleration.Z = 0;
2748
2749 _velocity.X = 0;
2750 _velocity.Y = 0;
2751 _velocity.Z = 0;
2752 m_rotationalVelocity.X = 0;
2753 m_rotationalVelocity.Y = 0;
2754 m_rotationalVelocity.Z = 0;
2755
2756 if (_parent == null)
2757 base.RequestPhysicsterseUpdate();
2758
2759 m_throttleUpdates = false;
2760 throttleCounter = 0;
2761 _zeroFlag = true;
2762 //outofBounds = true;
2763 }
2764
2765 //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation));
2766//Console.WriteLine("Adiff " + m_primName + " = " + Adiff);
2767 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
2768 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
2769 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
2770// && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01))
2771 && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large
2772 {
2773 _zeroFlag = true;
2774//Console.WriteLine("ZFT 2");
2775 m_throttleUpdates = false;
2776 }
2777 else
2778 {
2779 //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
2780 _zeroFlag = false;
2781 m_lastUpdateSent = false;
2782 //m_throttleUpdates = false;
2783 }
2784
2785 if (_zeroFlag)
2786 {
2787 _velocity.X = 0.0f;
2788 _velocity.Y = 0.0f;
2789 _velocity.Z = 0.0f;
2790
2791 _acceleration.X = 0;
2792 _acceleration.Y = 0;
2793 _acceleration.Z = 0;
2794
2795 //_orientation.w = 0f;
2796 //_orientation.X = 0f;
2797 //_orientation.Y = 0f;
2798 //_orientation.Z = 0f;
2799 m_rotationalVelocity.X = 0;
2800 m_rotationalVelocity.Y = 0;
2801 m_rotationalVelocity.Z = 0;
2802 if (!m_lastUpdateSent)
2803 {
2804 m_throttleUpdates = false;
2805 throttleCounter = 0;
2806 m_rotationalVelocity = pv;
2807
2808 if (_parent == null)
2809 {
2810 base.RequestPhysicsterseUpdate();
2811 }
2812
2813 m_lastUpdateSent = true;
2814 }
2815 }
2816 else
2817 {
2818 if (lastZeroFlag != _zeroFlag)
2819 {
2820 if (_parent == null)
2821 {
2822 base.RequestPhysicsterseUpdate();
2823 }
2824 }
2825
2826 m_lastVelocity = _velocity;
2827
2828 _position = l_position;
2829
2830 _velocity.X = vel.X;
2831 _velocity.Y = vel.Y;
2832 _velocity.Z = vel.Z;
2833
2834 _acceleration = ((_velocity - m_lastVelocity) / 0.1f);
2835 _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f);
2836 //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString());
2837
2838// if (_velocity.ApproxEquals(pv, 0.5f)) ???? Disregard rotational vel if lin vel is < 0.5 ?????
2839// {
2840// m_rotationalVelocity = pv;/
2841
2842// }
2843// else
2844// {
2845 m_rotationalVelocity = new Vector3(rotvel.X, rotvel.Y, rotvel.Z);
2846// }
2847
2848 //m_log.Debug("ODE: " + m_rotationalVelocity.ToString());
2849 _orientation.X = ori.X;
2850 _orientation.Y = ori.Y;
2851 _orientation.Z = ori.Z;
2852 _orientation.W = ori.W;
2853 m_lastUpdateSent = false;
2854 if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
2855 {
2856 if (_parent == null)
2857 {
2858 base.RequestPhysicsterseUpdate();
2859 }
2860 }
2861 else
2862 {
2863 throttleCounter++;
2864 }
2865 }
2866 m_lastposition = l_position;
2867 }
2868 else
2869 {
2870 // Not a body.. so Make sure the client isn't interpolating
2871 _velocity.X = 0;
2872 _velocity.Y = 0;
2873 _velocity.Z = 0;
2874
2875 _acceleration.X = 0;
2876 _acceleration.Y = 0;
2877 _acceleration.Z = 0;
2878
2879 m_rotationalVelocity.X = 0;
2880 m_rotationalVelocity.Y = 0;
2881 m_rotationalVelocity.Z = 0;
2882 _zeroFlag = true;
2883 }
2884 }
2885 }
2886
2887 public override bool FloatOnWater
2888 {
2889 set {
2890 m_taintCollidesWater = value;
2891 _parent_scene.AddPhysicsActorTaint(this);
2892 }
2893 }
2894
2895 public override void SetMomentum(Vector3 momentum)
2896 {
2897 }
2898
2899 public override Vector3 PIDTarget
2900 {
2901 set
2902 {
2903 if (value.IsFinite())
2904 {
2905 m_PIDTarget = value;
2906 }
2907 else
2908 m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object");
2909 }
2910 }
2911 public override bool PIDActive { set { m_usePID = value; } }
2912 public override float PIDTau { set { m_PIDTau = value; } }
2913
2914 // For RotLookAt
2915 public override Quaternion APIDTarget { set { m_APIDTarget = value; } }
2916 public override bool APIDActive { set { m_useAPID = value; } }
2917 public override float APIDStrength { set { m_APIDStrength = value; } }
2918 public override float APIDDamping { set { m_APIDDamping = value; } }
2919
2920 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
2921 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
2922 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
2923 public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
2924
2925 private void createAMotor(Vector3 axis)
2926 {
2927 if (Body == IntPtr.Zero)
2928 return;
2929
2930 if (Amotor != IntPtr.Zero)
2931 {
2932 d.JointDestroy(Amotor);
2933 Amotor = IntPtr.Zero;
2934 }
2935
2936 float axisnum = 3;
2937
2938 axisnum = (axisnum - (axis.X + axis.Y + axis.Z));
2939
2940 // PhysicsVector totalSize = new PhysicsVector(_size.X, _size.Y, _size.Z);
2941
2942
2943 // Inverse Inertia Matrix, set the X, Y, and/r Z inertia to 0 then invert it again.
2944 d.Mass objMass;
2945 d.MassSetZero(out objMass);
2946 DMassCopy(ref pMass, ref objMass);
2947
2948 //m_log.DebugFormat("1-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
2949
2950 Matrix4 dMassMat = FromDMass(objMass);
2951
2952 Matrix4 mathmat = Inverse(dMassMat);
2953
2954 /*
2955 //m_log.DebugFormat("2-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", mathmat[0, 0], mathmat[0, 1], mathmat[0, 2], mathmat[1, 0], mathmat[1, 1], mathmat[1, 2], mathmat[2, 0], mathmat[2, 1], mathmat[2, 2]);
2956
2957 mathmat = Inverse(mathmat);
2958
2959
2960 objMass = FromMatrix4(mathmat, ref objMass);
2961 //m_log.DebugFormat("3-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
2962
2963 mathmat = Inverse(mathmat);
2964 */
2965 if (axis.X == 0)
2966 {
2967 mathmat.M33 = 50.0000001f;
2968 //objMass.I.M22 = 0;
2969 }
2970 if (axis.Y == 0)
2971 {
2972 mathmat.M22 = 50.0000001f;
2973 //objMass.I.M11 = 0;
2974 }
2975 if (axis.Z == 0)
2976 {
2977 mathmat.M11 = 50.0000001f;
2978 //objMass.I.M00 = 0;
2979 }
2980
2981
2982
2983 mathmat = Inverse(mathmat);
2984 objMass = FromMatrix4(mathmat, ref objMass);
2985 //m_log.DebugFormat("4-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22);
2986
2987 //return;
2988 if (d.MassCheck(ref objMass))
2989 {
2990 d.BodySetMass(Body, ref objMass);
2991 }
2992 else
2993 {
2994 //m_log.Debug("[PHYSICS]: Mass invalid, ignoring");
2995 }
2996
2997 if (axisnum <= 0)
2998 return;
2999 // int dAMotorEuler = 1;
3000
3001 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
3002 d.JointAttach(Amotor, Body, IntPtr.Zero);
3003 d.JointSetAMotorMode(Amotor, 0);
3004
3005 d.JointSetAMotorNumAxes(Amotor,(int)axisnum);
3006 int i = 0;
3007
3008 if (axis.X == 0)
3009 {
3010 d.JointSetAMotorAxis(Amotor, i, 0, 1, 0, 0);
3011 i++;
3012 }
3013
3014 if (axis.Y == 0)
3015 {
3016 d.JointSetAMotorAxis(Amotor, i, 0, 0, 1, 0);
3017 i++;
3018 }
3019
3020 if (axis.Z == 0)
3021 {
3022 d.JointSetAMotorAxis(Amotor, i, 0, 0, 0, 1);
3023 i++;
3024 }
3025
3026 for (int j = 0; j < (int)axisnum; j++)
3027 {
3028 //d.JointSetAMotorAngle(Amotor, j, 0);
3029 }
3030
3031 //d.JointSetAMotorAngle(Amotor, 1, 0);
3032 //d.JointSetAMotorAngle(Amotor, 2, 0);
3033
3034 // These lowstops and high stops are effectively (no wiggle room)
3035 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0f);
3036 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f);
3037 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0f);
3038 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f);
3039 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f);
3040 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f);
3041 //d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 9000f);
3042 d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
3043 d.JointSetAMotorParam(Amotor, (int)dParam.FMax, Mass * 50f);//
3044
3045 } 2397 }
3046 2398/* No one uses this?
3047 public Matrix4 FromDMass(d.Mass pMass) 2399 public Matrix4 FromDMass(d.Mass pMass)
3048 { 2400 {
3049 Matrix4 obj; 2401 Matrix4 obj;
@@ -3065,7 +2417,7 @@ Console.WriteLine(" JointCreateFixed");
3065 obj.M44 = 1; 2417 obj.M44 = 1;
3066 return obj; 2418 return obj;
3067 } 2419 }
3068 2420*/
3069 public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj) 2421 public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj)
3070 { 2422 {
3071 obj.I.M00 = pMat[0, 0]; 2423 obj.I.M00 = pMat[0, 0];
@@ -3090,7 +2442,7 @@ Console.WriteLine(" JointCreateFixed");
3090 { 2442 {
3091 _parent_scene.remCollisionEventReporting(this); 2443 _parent_scene.remCollisionEventReporting(this);
3092 m_eventsubscription = 0; 2444 m_eventsubscription = 0;
3093 } 2445 }
3094 2446
3095 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) 2447 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
3096 { 2448 {
@@ -3292,5 +2644,1107 @@ Console.WriteLine(" JointCreateFixed");
3292 m_material = pMaterial; 2644 m_material = pMaterial;
3293 } 2645 }
3294 2646
3295 } 2647 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
2648 {
2649 switch (pParam)
2650 {
2651 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
2652 if (pValue < 0.01f) pValue = 0.01f;
2653 // m_angularDeflectionEfficiency = pValue;
2654 break;
2655 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
2656 if (pValue < 0.01f) pValue = 0.01f;
2657 // m_angularDeflectionTimescale = pValue;
2658 break;
2659 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
2660 if (pValue < 0.01f) pValue = 0.01f;
2661 m_angularMotorDecayTimescale = pValue;
2662 break;
2663 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
2664 if (pValue < 0.01f) pValue = 0.01f;
2665 m_angularMotorTimescale = pValue;
2666 break;
2667 case Vehicle.BANKING_EFFICIENCY:
2668 if (pValue < 0.01f) pValue = 0.01f;
2669 // m_bankingEfficiency = pValue;
2670 break;
2671 case Vehicle.BANKING_MIX:
2672 if (pValue < 0.01f) pValue = 0.01f;
2673 // m_bankingMix = pValue;
2674 break;
2675 case Vehicle.BANKING_TIMESCALE:
2676 if (pValue < 0.01f) pValue = 0.01f;
2677 // m_bankingTimescale = pValue;
2678 break;
2679 case Vehicle.BUOYANCY:
2680 if (pValue < -1f) pValue = -1f;
2681 if (pValue > 1f) pValue = 1f;
2682 m_VehicleBuoyancy = pValue;
2683 break;
2684// case Vehicle.HOVER_EFFICIENCY:
2685// if (pValue < 0f) pValue = 0f;
2686// if (pValue > 1f) pValue = 1f;
2687// m_VhoverEfficiency = pValue;
2688// break;
2689 case Vehicle.HOVER_HEIGHT:
2690 m_VhoverHeight = pValue;
2691 break;
2692 case Vehicle.HOVER_TIMESCALE:
2693 if (pValue < 0.01f) pValue = 0.01f;
2694 m_VhoverTimescale = pValue;
2695 break;
2696 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
2697 if (pValue < 0.01f) pValue = 0.01f;
2698 // m_linearDeflectionEfficiency = pValue;
2699 break;
2700 case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
2701 if (pValue < 0.01f) pValue = 0.01f;
2702 // m_linearDeflectionTimescale = pValue;
2703 break;
2704 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
2705 if (pValue < 0.01f) pValue = 0.01f;
2706 m_linearMotorDecayTimescale = pValue;
2707 break;
2708 case Vehicle.LINEAR_MOTOR_TIMESCALE:
2709 if (pValue < 0.01f) pValue = 0.01f;
2710 m_linearMotorTimescale = pValue;
2711 break;
2712 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
2713 if (pValue < 0.1f) pValue = 0.1f; // Less goes unstable
2714 if (pValue > 1.0f) pValue = 1.0f;
2715 m_verticalAttractionEfficiency = pValue;
2716 break;
2717 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
2718 if (pValue < 0.01f) pValue = 0.01f;
2719 m_verticalAttractionTimescale = pValue;
2720 break;
2721
2722 // These are vector properties but the engine lets you use a single float value to
2723 // set all of the components to the same value
2724 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
2725 if (pValue > 30f) pValue = 30f;
2726 if (pValue < 0.1f) pValue = 0.1f;
2727 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
2728 break;
2729 case Vehicle.ANGULAR_MOTOR_DIRECTION:
2730 m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
2731 UpdateAngDecay();
2732 break;
2733 case Vehicle.LINEAR_FRICTION_TIMESCALE:
2734 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
2735 break;
2736 case Vehicle.LINEAR_MOTOR_DIRECTION:
2737 m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
2738 UpdateLinDecay();
2739 break;
2740 case Vehicle.LINEAR_MOTOR_OFFSET:
2741 // m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
2742 break;
2743
2744 }
2745
2746 }//end ProcessFloatVehicleParam
2747
2748 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
2749 {
2750 switch (pParam)
2751 {
2752 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
2753 if (pValue.X > 30f) pValue.X = 30f;
2754 if (pValue.X < 0.1f) pValue.X = 0.1f;
2755 if (pValue.Y > 30f) pValue.Y = 30f;
2756 if (pValue.Y < 0.1f) pValue.Y = 0.1f;
2757 if (pValue.Z > 30f) pValue.Z = 30f;
2758 if (pValue.Z < 0.1f) pValue.Z = 0.1f;
2759 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
2760 break;
2761 case Vehicle.ANGULAR_MOTOR_DIRECTION:
2762 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
2763 // Limit requested angular speed to 2 rps= 4 pi rads/sec
2764 if(m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f;
2765 if(m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f;
2766 if(m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f;
2767 if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f;
2768 if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f;
2769 if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f;
2770 UpdateAngDecay();
2771 break;
2772 case Vehicle.LINEAR_FRICTION_TIMESCALE:
2773 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
2774 break;
2775 case Vehicle.LINEAR_MOTOR_DIRECTION:
2776 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); // velocity requested by LSL, for max limiting
2777 UpdateLinDecay();
2778 break;
2779 case Vehicle.LINEAR_MOTOR_OFFSET:
2780 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
2781 break;
2782 }
2783
2784 }//end ProcessVectorVehicleParam
2785
2786 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
2787 {
2788 switch (pParam)
2789 {
2790 case Vehicle.REFERENCE_FRAME:
2791 // m_referenceFrame = pValue;
2792 break;
2793 }
2794
2795 }//end ProcessRotationVehicleParam
2796
2797 internal void ProcessVehicleFlags(int pParam, bool remove)
2798 {
2799 if (remove)
2800 {
2801 m_flags &= ~((VehicleFlag)pParam);
2802 }
2803 else
2804 {
2805 m_flags |= (VehicleFlag)pParam;
2806 }
2807 }
2808
2809 internal void ProcessTypeChange(Vehicle pType)
2810 {
2811 // Set Defaults For Type
2812 m_type = pType;
2813 switch (pType)
2814 {
2815 case Vehicle.TYPE_SLED:
2816 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
2817 m_angularFrictionTimescale = new Vector3(30, 30, 30);
2818// m_lLinMotorVel = Vector3.Zero;
2819 m_linearMotorTimescale = 1000;
2820 m_linearMotorDecayTimescale = 120;
2821 m_angularMotorDirection = Vector3.Zero;
2822 m_angularMotorDVel = Vector3.Zero;
2823 m_angularMotorTimescale = 1000;
2824 m_angularMotorDecayTimescale = 120;
2825 m_VhoverHeight = 0;
2826// m_VhoverEfficiency = 1;
2827 m_VhoverTimescale = 10;
2828 m_VehicleBuoyancy = 0;
2829 // m_linearDeflectionEfficiency = 1;
2830 // m_linearDeflectionTimescale = 1;
2831 // m_angularDeflectionEfficiency = 1;
2832 // m_angularDeflectionTimescale = 1000;
2833 // m_bankingEfficiency = 0;
2834 // m_bankingMix = 1;
2835 // m_bankingTimescale = 10;
2836 // m_referenceFrame = Quaternion.Identity;
2837 m_flags &=
2838 ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
2839 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
2840 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
2841 break;
2842 case Vehicle.TYPE_CAR:
2843 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
2844 m_angularFrictionTimescale = new Vector3(30, 30, 30); // was 1000, but sl max frict time is 30.
2845// m_lLinMotorVel = Vector3.Zero;
2846 m_linearMotorTimescale = 1;
2847 m_linearMotorDecayTimescale = 60;
2848 m_angularMotorDirection = Vector3.Zero;
2849 m_angularMotorDVel = Vector3.Zero;
2850 m_angularMotorTimescale = 1;
2851 m_angularMotorDecayTimescale = 0.8f;
2852 m_VhoverHeight = 0;
2853// m_VhoverEfficiency = 0;
2854 m_VhoverTimescale = 1000;
2855 m_VehicleBuoyancy = 0;
2856 // // m_linearDeflectionEfficiency = 1;
2857 // // m_linearDeflectionTimescale = 2;
2858 // // m_angularDeflectionEfficiency = 0;
2859 // m_angularDeflectionTimescale = 10;
2860 m_verticalAttractionEfficiency = 1f;
2861 m_verticalAttractionTimescale = 10f;
2862 // m_bankingEfficiency = -0.2f;
2863 // m_bankingMix = 1;
2864 // m_bankingTimescale = 1;
2865 // m_referenceFrame = Quaternion.Identity;
2866 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
2867 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY |
2868 VehicleFlag.LIMIT_MOTOR_UP);
2869 break;
2870 case Vehicle.TYPE_BOAT:
2871 m_linearFrictionTimescale = new Vector3(10, 3, 2);
2872 m_angularFrictionTimescale = new Vector3(10,10,10);
2873// m_lLinMotorVel = Vector3.Zero;
2874 m_linearMotorTimescale = 5;
2875 m_linearMotorDecayTimescale = 60;
2876 m_angularMotorDirection = Vector3.Zero;
2877 m_angularMotorDVel = Vector3.Zero;
2878 m_angularMotorTimescale = 4;
2879 m_angularMotorDecayTimescale = 4;
2880 m_VhoverHeight = 0;
2881// m_VhoverEfficiency = 0.5f;
2882 m_VhoverTimescale = 2;
2883 m_VehicleBuoyancy = 1;
2884 // m_linearDeflectionEfficiency = 0.5f;
2885 // m_linearDeflectionTimescale = 3;
2886 // m_angularDeflectionEfficiency = 0.5f;
2887 // m_angularDeflectionTimescale = 5;
2888 m_verticalAttractionEfficiency = 0.5f;
2889 m_verticalAttractionTimescale = 5f;
2890 // m_bankingEfficiency = -0.3f;
2891 // m_bankingMix = 0.8f;
2892 // m_bankingTimescale = 1;
2893 // m_referenceFrame = Quaternion.Identity;
2894 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY |
2895 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
2896 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY |
2897 VehicleFlag.LIMIT_MOTOR_UP);
2898 break;
2899 case Vehicle.TYPE_AIRPLANE:
2900 m_linearFrictionTimescale = new Vector3(200, 10, 5);
2901 m_angularFrictionTimescale = new Vector3(20, 20, 20);
2902// m_lLinMotorVel = Vector3.Zero;
2903 m_linearMotorTimescale = 2;
2904 m_linearMotorDecayTimescale = 60;
2905 m_angularMotorDirection = Vector3.Zero;
2906 m_angularMotorDVel = Vector3.Zero;
2907 m_angularMotorTimescale = 4;
2908 m_angularMotorDecayTimescale = 4;
2909 m_VhoverHeight = 0;
2910// m_VhoverEfficiency = 0.5f;
2911 m_VhoverTimescale = 1000;
2912 m_VehicleBuoyancy = 0;
2913 // m_linearDeflectionEfficiency = 0.5f;
2914 // m_linearDeflectionTimescale = 3;
2915 // m_angularDeflectionEfficiency = 1;
2916 // m_angularDeflectionTimescale = 2;
2917 m_verticalAttractionEfficiency = 0.9f;
2918 m_verticalAttractionTimescale = 2f;
2919 // m_bankingEfficiency = 1;
2920 // m_bankingMix = 0.7f;
2921 // m_bankingTimescale = 2;
2922 // m_referenceFrame = Quaternion.Identity;
2923 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
2924 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
2925 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
2926 break;
2927 case Vehicle.TYPE_BALLOON:
2928 m_linearFrictionTimescale = new Vector3(5, 5, 5);
2929 m_angularFrictionTimescale = new Vector3(10, 10, 10);
2930 m_linearMotorTimescale = 5;
2931 m_linearMotorDecayTimescale = 60;
2932 m_angularMotorDirection = Vector3.Zero;
2933 m_angularMotorDVel = Vector3.Zero;
2934 m_angularMotorTimescale = 6;
2935 m_angularMotorDecayTimescale = 10;
2936 m_VhoverHeight = 5;
2937// m_VhoverEfficiency = 0.8f;
2938 m_VhoverTimescale = 10;
2939 m_VehicleBuoyancy = 1;
2940 // m_linearDeflectionEfficiency = 0;
2941 // m_linearDeflectionTimescale = 5;
2942 // m_angularDeflectionEfficiency = 0;
2943 // m_angularDeflectionTimescale = 5;
2944 m_verticalAttractionEfficiency = 1f;
2945 m_verticalAttractionTimescale = 100f;
2946 // m_bankingEfficiency = 0;
2947 // m_bankingMix = 0.7f;
2948 // m_bankingTimescale = 5;
2949 // m_referenceFrame = Quaternion.Identity;
2950 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
2951 VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
2952 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
2953 break;
2954
2955 }
2956 }//end SetDefaultsForType
2957
2958 internal void Enable(IntPtr pBody, OdeScene pParentScene)
2959 {
2960 if (m_type == Vehicle.TYPE_NONE)
2961 return;
2962
2963 m_body = pBody;
2964 }
2965
2966
2967 internal void Halt()
2968 { // Kill all motions, when non-physical
2969 m_linearMotorDirection = Vector3.Zero;
2970 m_lLinMotorDVel = Vector3.Zero;
2971 m_lLinObjectVel = Vector3.Zero;
2972 m_wLinObjectVel = Vector3.Zero;
2973 m_angularMotorDirection = Vector3.Zero;
2974 m_lastAngularVelocity = Vector3.Zero;
2975 m_angularMotorDVel = Vector3.Zero;
2976 }
2977
2978 private void UpdateLinDecay()
2979 {
2980 if (Math.Abs(m_linearMotorDirection.X) > Math.Abs(m_lLinMotorDVel.X)) m_lLinMotorDVel.X = m_linearMotorDirection.X;
2981 if (Math.Abs(m_linearMotorDirection.Y) > Math.Abs(m_lLinMotorDVel.Y)) m_lLinMotorDVel.Y = m_linearMotorDirection.Y;
2982 if (Math.Abs(m_linearMotorDirection.Z) > Math.Abs(m_lLinMotorDVel.Z)) m_lLinMotorDVel.Z = m_linearMotorDirection.Z;
2983 } // else let the motor decay on its own
2984
2985 private void UpdateAngDecay()
2986 {
2987 if (Math.Abs(m_angularMotorDirection.X) > Math.Abs(m_angularMotorDVel.X)) m_angularMotorDVel.X = m_angularMotorDirection.X;
2988 if (Math.Abs(m_angularMotorDirection.Y) > Math.Abs(m_angularMotorDVel.Y)) m_angularMotorDVel.Y = m_angularMotorDirection.Y;
2989 if (Math.Abs(m_angularMotorDirection.Z) > Math.Abs(m_angularMotorDVel.Z)) m_angularMotorDVel.Z = m_angularMotorDirection.Z;
2990 } // else let the motor decay on its own
2991
2992 public void Move(float timestep)
2993 {
2994 float fx = 0;
2995 float fy = 0;
2996 float fz = 0;
2997
2998 frcount++; // used to limit debug comment output
2999 if (frcount > 100)
3000 frcount = 0;
3001
3002 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims.
3003 {
3004
3005// Old public void UpdatePositionAndVelocity(), more accuratley calculated here
3006 bool lastZeroFlag = _zeroFlag; // was it stopped
3007 d.Vector3 vec = d.BodyGetPosition(Body);
3008 d.Quaternion ori = d.BodyGetQuaternion(Body);
3009 d.Vector3 vel = d.BodyGetLinearVel(Body);
3010 // d.Vector3 rotvel = d.BodyGetAngularVel(Body);
3011 d.Vector3 torque = d.BodyGetTorque(Body);
3012 _torque = new Vector3(torque.X, torque.Y, torque.Z);
3013 Vector3 l_position = Vector3.Zero;
3014 Quaternion l_orientation = Quaternion.Identity;
3015
3016 m_lastposition = _position;
3017 m_lastorientation = _orientation;
3018
3019 l_position.X = vec.X;
3020 l_position.Y = vec.Y;
3021 l_position.Z = vec.Z;
3022 l_orientation.X = ori.X;
3023 l_orientation.Y = ori.Y;
3024 l_orientation.Z = ori.Z;
3025 l_orientation.W = ori.W;
3026//Console.WriteLine("Move {0} at {1}", m_primName, l_position);
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
3047
3048 if (l_position.Z < 0)
3049 {
3050 // This is so prim that get lost underground don't fall forever and suck up
3051 //
3052 // Sim resources and memory.
3053 // Disables the prim's movement physics....
3054 // It's a hack and will generate a console message if it fails.
3055
3056 //IsPhysical = false;
3057 if (_parent == null) base.RaiseOutOfBounds(_position);
3058
3059 _acceleration.X = 0; // This stuff may stop client display but it has no
3060 _acceleration.Y = 0; // effect on the object in phys engine!
3061 _acceleration.Z = 0;
3062
3063 _velocity.X = 0;
3064 _velocity.Y = 0;
3065 _velocity.Z = 0;
3066 m_rotationalVelocity.X = 0;
3067 m_rotationalVelocity.Y = 0;
3068 m_rotationalVelocity.Z = 0;
3069
3070 if (_parent == null) base.RequestPhysicsterseUpdate();
3071
3072 m_throttleUpdates = false;
3073 throttleCounter = 0;
3074 _zeroFlag = true;
3075 //outofBounds = true;
3076 } // end neg Z check
3077
3078 // Is it moving?
3079 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
3080 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
3081 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
3082 && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large
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 }
3095
3096 if (_zeroFlag)
3097 { // Its stopped
3098 _velocity.X = 0.0f;
3099 _velocity.Y = 0.0f;
3100 _velocity.Z = 0.0f;
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 {
3114 m_throttleUpdates = false;
3115 throttleCounter = 0;
3116 if (_parent == null)
3117 {
3118 base.RequestPhysicsterseUpdate();
3119 }
3120
3121 m_lastUpdateSent = true;
3122 }
3123 }
3124 else
3125 { // Its moving
3126 if (lastZeroFlag != _zeroFlag)
3127 {
3128 if (_parent == null)
3129 {
3130 base.RequestPhysicsterseUpdate();
3131 }
3132 }
3133
3134 m_lastVelocity = _velocity;
3135
3136 _position = l_position;
3137
3138 _velocity.X = vel.X;
3139 _velocity.Y = vel.Y;
3140 _velocity.Z = vel.Z;
3141// Why 2 calcs???
3142// _acceleration = ((_velocity - m_lastVelocity) / 0.1f);
3143// _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f,
3144// _velocity.Y - m_lastVelocity.Y / 0.1f,
3145// _velocity.Z - m_lastVelocity.Z / 0.1f);
3146
3147 _acceleration = ((_velocity - m_lastVelocity) / timestep);
3148
3149 _orientation.X = ori.X;
3150 _orientation.Y = ori.Y;
3151 _orientation.Z = ori.Z;
3152 _orientation.W = ori.W;
3153 m_lastUpdateSent = false;
3154 if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
3155 {
3156 if (_parent == null)
3157 {
3158 base.RequestPhysicsterseUpdate();
3159 }
3160 }
3161 else
3162 {
3163 throttleCounter++;
3164 }
3165 }
3166 m_lastposition = l_position;
3167
3168 /// End of old UpdatePositionAndVelocity insert
3169
3170//if (!Acceleration.ApproxEquals(Vector3.Zero, 0.01f)) Console.WriteLine("Move " + m_primName + " Accel=" + Acceleration);
3171// if(frcount == 0) Console.WriteLine("Move " + m_primName + " VTyp " + m_type +
3172// " usePID=" + m_usePID + " seHover=" + m_useHoverPID + " useAPID=" + m_useAPID);
3173 if (m_type != Vehicle.TYPE_NONE)
3174 {
3175 // get body attitude
3176 d.Quaternion rot = d.BodyGetQuaternion(Body);
3177 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
3178 Quaternion irotq = Quaternion.Inverse(rotq);
3179
3180 // VEHICLE Linear Motion
3181 d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame
3182 Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z);
3183 m_lLinObjectVel = vel_now * irotq;
3184
3185 if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate
3186 {
3187 if ( Vector3.Mag(m_lLinMotorDVel) < 1.0f)
3188 {
3189 float decayfactor = m_linearMotorDecayTimescale/timestep;
3190 Vector3 decayAmount = (m_lLinMotorDVel/decayfactor);
3191 m_lLinMotorDVel -= decayAmount;
3192 }
3193 else
3194 {
3195 float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale)));
3196 Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * timestep;
3197 m_lLinMotorDVel -= decel;
3198 }
3199 if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
3200 {
3201 m_lLinMotorDVel = Vector3.Zero;
3202 }
3203 else
3204 {
3205 if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X;
3206 if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y;
3207 if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z;
3208 }
3209 } // end linear motor decay
3210
3211 if ( (! m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) )
3212 {
3213 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
3214 if (m_linearMotorTimescale < 300.0f)
3215 {
3216 Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel;
3217 float linfactor = m_linearMotorTimescale/timestep;
3218 Vector3 attackAmount = (attack_error/linfactor) * 1.3f;
3219 m_lLinObjectVel += attackAmount;
3220 }
3221 if (m_linearFrictionTimescale.X < 300.0f)
3222 {
3223 float fricfactor = m_linearFrictionTimescale.X / timestep;
3224 float fricX = m_lLinObjectVel.X / fricfactor;
3225 m_lLinObjectVel.X -= fricX;
3226 }
3227 if (m_linearFrictionTimescale.Y < 300.0f)
3228 {
3229 float fricfactor = m_linearFrictionTimescale.Y / timestep;
3230 float fricY = m_lLinObjectVel.Y / fricfactor;
3231 m_lLinObjectVel.Y -= fricY;
3232 }
3233 if (m_linearFrictionTimescale.Z < 300.0f)
3234 {
3235 float fricfactor = m_linearFrictionTimescale.Z / timestep;
3236//if(frcount == 0) Console.WriteLine("Zfric={0}", fricfactor);
3237 float fricZ = m_lLinObjectVel.Z / fricfactor;
3238 m_lLinObjectVel.Z -= fricZ;
3239 }
3240 }
3241 m_wLinObjectVel = m_lLinObjectVel * rotq;
3242
3243 // Gravity and Buoyancy
3244 Vector3 grav = Vector3.Zero;
3245 if(m_VehicleBuoyancy < 1.0f)
3246 {
3247 // There is some gravity, make a gravity force vector
3248 // that is applied after object velocity.
3249 d.Mass objMass;
3250 d.BodyGetMass(Body, out objMass);
3251 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
3252 grav.Z = _parent_scene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force
3253 } // else its 1.0, no gravity.
3254
3255 // Hovering
3256 if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
3257 {
3258 // We should hover, get the target height
3259 d.Vector3 pos = d.BodyGetPosition(Body);
3260 if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY)
3261 {
3262 m_VhoverTargetHeight = _parent_scene.GetWaterLevel() + m_VhoverHeight;
3263 }
3264 else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY)
3265 {
3266 m_VhoverTargetHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
3267 }
3268 else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT)
3269 {
3270 m_VhoverTargetHeight = m_VhoverHeight;
3271 }
3272
3273 if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY)
3274 {
3275 // If body is aready heigher, use its height as target height
3276 if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
3277 }
3278
3279// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
3280// m_VhoverTimescale = 0f; // time to acheive height
3281// timestep is time since last frame,in secs
3282 float herr0 = pos.Z - m_VhoverTargetHeight;
3283 // Replace Vertical speed with correction figure if significant
3284 if(Math.Abs(herr0) > 0.01f )
3285 {
3286 //? d.Mass objMass;
3287 //? d.BodyGetMass(Body, out objMass);
3288 m_wLinObjectVel.Z = - ( (herr0 * timestep * 50.0f) / m_VhoverTimescale);
3289 //KF: m_VhoverEfficiency is not yet implemented
3290 }
3291 else
3292 {
3293 m_wLinObjectVel.Z = 0f;
3294 }
3295 }
3296 else
3297 { // not hovering, Gravity rules
3298 m_wLinObjectVel.Z = vel_now.Z;
3299 }
3300
3301
3302 // Vehicle Linear Motion done =======================================
3303 // Apply velocity
3304 d.BodySetLinearVel(Body, m_wLinObjectVel.X, m_wLinObjectVel.Y, m_wLinObjectVel.Z);
3305 // apply gravity force
3306 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
3307//if(frcount == 0) Console.WriteLine("Grav {0}", grav);
3308 // end MoveLinear()
3309
3310
3311 // MoveAngular
3312 /*
3313 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
3314
3315 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
3316 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
3317 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
3318
3319 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
3320 private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body
3321 */
3322//if(frcount == 0) Console.WriteLine("MoveAngular ");
3323
3324 d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body);
3325 Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z);
3326 angObjectVel = angObjectVel * irotq; // ============ Converts to LOCAL rotation
3327
3328//if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel);
3329
3330 // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack.
3331 float atk_decayfactor = 23.0f / (m_angularMotorTimescale * timestep);
3332 m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor;
3333 // Decay Angular Motor 2.
3334 if (m_angularMotorDecayTimescale < 300.0f)
3335 {
3336 if ( Vector3.Mag(m_angularMotorDVel) < 1.0f)
3337 {
3338 float decayfactor = (m_angularMotorDecayTimescale)/timestep;
3339 Vector3 decayAmount = (m_angularMotorDVel/decayfactor);
3340 m_angularMotorDVel -= decayAmount;
3341 }
3342 else
3343 {
3344 Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * timestep / m_angularMotorDecayTimescale;
3345 m_angularMotorDVel -= decel;
3346 }
3347
3348 if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
3349 {
3350 m_angularMotorDVel = Vector3.Zero;
3351 }
3352 else
3353 {
3354 if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X;
3355 if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y;
3356 if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z;
3357 }
3358 } // end decay angular motor
3359//if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel);
3360
3361//if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel);
3362
3363 // Vertical attractor section
3364 Vector3 vertattr = Vector3.Zero;
3365
3366 if(m_verticalAttractionTimescale < 300)
3367 {
3368 float VAservo = 1.0f / (m_verticalAttractionTimescale * timestep);
3369 // make a vector pointing up
3370 Vector3 verterr = Vector3.Zero;
3371 verterr.Z = 1.0f;
3372 // rotate it to Body Angle
3373 verterr = verterr * rotq;
3374 // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1.
3375 // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go
3376 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
3377
3378 if (verterr.Z < 0.0f)
3379 { // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to
3380 // vertical, BUT for some reason a z-rotation is imparted to the object. TBI.
3381//Console.WriteLine("InvertFlip");
3382 verterr.X = 2.0f - verterr.X;
3383 verterr.Y = 2.0f - verterr.Y;
3384 }
3385 verterr *= 0.5f;
3386 // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt)
3387
3388 if ((!angObjectVel.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f))
3389 {
3390 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
3391 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
3392 vertattr.X = verterr.Y;
3393 vertattr.Y = - verterr.X;
3394 vertattr.Z = 0f;
3395//if(frcount == 0) Console.WriteLine("VAerr=" + verterr);
3396
3397 // scaling appears better usingsquare-law
3398 float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
3399 float bounce = 1.0f - damped;
3400 // 0 = crit damp, 1 = bouncy
3401 float oavz = angObjectVel.Z; // retain z velocity
3402 // time-scaled correction, which sums, therefore is bouncy:
3403 angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce;
3404 // damped, good @ < 90:
3405 angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped);
3406 angObjectVel.Z = oavz;
3407//if(frcount == 0) Console.WriteLine("VA+");
3408//Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel);
3409 }
3410 else
3411 {
3412 // else error is very small
3413 angObjectVel.X = 0f;
3414 angObjectVel.Y = 0f;
3415//if(frcount == 0) Console.WriteLine("VA0");
3416 }
3417 } // else vertical attractor is off
3418//if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel);
3419
3420 if ( (! m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) )
3421 { // if motor or object have motion
3422 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
3423
3424 if (m_angularMotorTimescale < 300.0f)
3425 {
3426 Vector3 attack_error = m_angularMotorDVel - angObjectVel;
3427 float angfactor = m_angularMotorTimescale/timestep;
3428 Vector3 attackAmount = (attack_error/angfactor);
3429 angObjectVel += attackAmount;
3430//if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount);
3431//if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel);
3432 }
3433
3434 angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep);
3435 angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep);
3436 angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep);
3437 } // else no signif. motion
3438
3439//if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel);
3440 // Bank section tba
3441 // Deflection section tba
3442//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel);
3443
3444 m_lastAngularVelocity = angObjectVel;
3445
3446 if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f))
3447 {
3448 if (m_angularEnable.X == 0)
3449 m_lastAngularVelocity.X = 0f;
3450 if (m_angularEnable.Y == 0)
3451 m_lastAngularVelocity.Y = 0f;
3452 if (m_angularEnable.Z == 0)
3453 m_lastAngularVelocity.Z = 0f;
3454 }
3455 // Apply to the body
3456// Vector3 aInc = m_lastAngularVelocity - initavel;
3457//if(frcount == 0) Console.WriteLine("Inc {0}", aInc);
3458 m_lastAngularVelocity = m_lastAngularVelocity * rotq; // ================ Converts to WORLD rotation
3459
3460 d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
3461//if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity);
3462
3463 } // end VEHICLES
3464 else
3465 {
3466 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
3467 // NON-'VEHICLES' are dealt with here
3468 /// Dynamics Angular Lock ========================================================================
3469 if (d.BodyIsEnabled(Body) && !m_angularEnable.ApproxEquals(Vector3.One, 0.003f))
3470 {
3471 d.Vector3 avel2 = d.BodyGetAngularVel(Body);
3472 if (m_angularEnable.X == 0)
3473 avel2.X = 0;
3474 if (m_angularEnable.Y == 0)
3475 avel2.Y = 0;
3476 if (m_angularEnable.Z == 0)
3477 avel2.Z = 0;
3478 d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
3479 }
3480
3481
3482 /// Dynamics Buoyancy ===============================================================================
3483 //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle.
3484 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
3485 // NB Prims in ODE are no subject to global gravity
3486 float m_mass = CalculateMass();
3487 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass
3488
3489 if (m_usePID)
3490 {
3491//if(frcount == 0) Console.WriteLine("PID " + m_primName);
3492 // KF - this is for object MoveToTarget.
3493
3494 //if (!d.BodyIsEnabled(Body))
3495 //d.BodySetForce(Body, 0f, 0f, 0f);
3496
3497 // no lock; for now it's only called from within Simulate()
3498
3499 // If the PID Controller isn't active then we set our force
3500 // calculating base velocity to the current position
3501
3502 if ((m_PIDTau < 1) && (m_PIDTau != 0))
3503 {
3504 //PID_G = PID_G / m_PIDTau;
3505 m_PIDTau = 1;
3506 }
3507
3508 if ((PID_G - m_PIDTau) <= 0)
3509 {
3510 PID_G = m_PIDTau + 1;
3511 }
3512 //PidStatus = true;
3513
3514 // PhysicsVector vec = new PhysicsVector();
3515// d.Vector3 vel = d.BodyGetLinearVel(Body);
3516
3517 d.Vector3 pos = d.BodyGetPosition(Body);
3518 _target_velocity =
3519 new Vector3(
3520 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
3521 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
3522 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
3523 );
3524
3525 // if velocity is zero, use position control; otherwise, velocity control
3526
3527 if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
3528 {
3529 // keep track of where we stopped. No more slippin' & slidin'
3530
3531 // We only want to deactivate the PID Controller if we think we want to have our surrogate
3532 // react to the physics scene by moving it's position.
3533 // Avatar to Avatar collisions
3534 // Prim to avatar collisions
3535
3536 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
3537 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
3538 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
3539 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3540 d.BodySetLinearVel(Body, 0, 0, 0);
3541 d.BodyAddForce(Body, 0, 0, fz);
3542 // return;
3543 }
3544 else
3545 {
3546 _zeroFlag = false;
3547
3548 // We're flying and colliding with something
3549 fx = ((_target_velocity.X) - vel.X) * (PID_D);
3550 fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
3551
3552 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
3553
3554 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
3555 }
3556 } // end if (m_usePID)
3557
3558 /// Dynamics Hover ===================================================================================
3559 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
3560 if (m_useHoverPID && !m_usePID)
3561 {
3562//Console.WriteLine("Hover " + m_primName);
3563
3564 // If we're using the PID controller, then we have no gravity
3565 fz = (-1 * _parent_scene.gravityz) * m_mass;
3566
3567 // no lock; for now it's only called from within Simulate()
3568
3569 // If the PID Controller isn't active then we set our force
3570 // calculating base velocity to the current position
3571
3572 if ((m_PIDTau < 1))
3573 {
3574 PID_G = PID_G / m_PIDTau;
3575 }
3576
3577 if ((PID_G - m_PIDTau) <= 0)
3578 {
3579 PID_G = m_PIDTau + 1;
3580 }
3581
3582
3583 // Where are we, and where are we headed?
3584 d.Vector3 pos = d.BodyGetPosition(Body);
3585// d.Vector3 vel = d.BodyGetLinearVel(Body);
3586
3587
3588 // Non-Vehicles have a limited set of Hover options.
3589 // determine what our target height really is based on HoverType
3590 switch (m_PIDHoverType)
3591 {
3592 case PIDHoverType.Ground:
3593 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3594 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3595 break;
3596 case PIDHoverType.GroundAndWater:
3597 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3598 m_waterHeight = _parent_scene.GetWaterLevel();
3599 if (m_groundHeight > m_waterHeight)
3600 {
3601 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3602 }
3603 else
3604 {
3605 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
3606 }
3607 break;
3608
3609 } // end switch (m_PIDHoverType)
3610
3611
3612 _target_velocity =
3613 new Vector3(0.0f, 0.0f,
3614 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
3615 );
3616
3617 // if velocity is zero, use position control; otherwise, velocity control
3618
3619 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
3620 {
3621 // keep track of where we stopped. No more slippin' & slidin'
3622
3623 // We only want to deactivate the PID Controller if we think we want to have our surrogate
3624 // react to the physics scene by moving it's position.
3625 // Avatar to Avatar collisions
3626 // Prim to avatar collisions
3627
3628 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
3629 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
3630 d.BodyAddForce(Body, 0, 0, fz);
3631 //KF this prevents furthur motions return;
3632 }
3633 else
3634 {
3635 _zeroFlag = false;
3636
3637 // We're flying and colliding with something
3638 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
3639 }
3640 } // end m_useHoverPID && !m_usePID
3641
3642 /// Dynamics RotLookAt =================================================================================
3643 if (m_useAPID)
3644 {
3645 // RotLookAt, apparently overrides all other rotation sources. Inputs:
3646 // Quaternion m_APIDTarget
3647 // float m_APIDStrength // From SL experiments, this is the time to get there
3648 // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly
3649 // Also in SL the mass of the object has no effect on time to get there.
3650 // Factors:
3651 // get present body rotation
3652 float limit = 1.0f;
3653 float scaler = 50f; // adjusts damping time
3654 float RLAservo = 0f;
3655
3656 d.Quaternion rot = d.BodyGetQuaternion(Body);
3657 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
3658 Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget;
3659 float diff_angle;
3660 Vector3 diff_axis;
3661 rot_diff.GetAxisAngle(out diff_axis, out diff_angle);
3662 diff_axis.Normalize();
3663 if(diff_angle > 0.01f) // diff_angle is always +ve
3664 {
3665// PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z);
3666 Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z);
3667 rotforce = rotforce * rotq;
3668 if(diff_angle > limit) diff_angle = limit; // cap the rotate rate
3669// RLAservo = timestep / m_APIDStrength * m_mass * scaler;
3670 // rotforce = rotforce * RLAservo * diff_angle ;
3671 // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z);
3672 RLAservo = timestep / m_APIDStrength * scaler;
3673 rotforce = rotforce * RLAservo * diff_angle ;
3674
3675 if (m_angularEnable.X == 0)
3676 rotforce.X = 0;
3677 if (m_angularEnable.Y == 0)
3678 rotforce.Y = 0;
3679 if (m_angularEnable.Z == 0)
3680 rotforce.Z = 0;
3681
3682 d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z);
3683//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo);
3684 }
3685//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle);
3686 } // end m_useAPID
3687
3688 /// Dynamics Apply Forces ===================================================================================
3689 fx *= m_mass;
3690 fy *= m_mass;
3691 //fz *= m_mass;
3692
3693 fx += m_force.X;
3694 fy += m_force.Y;
3695 fz += m_force.Z;
3696
3697 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
3698 if (fx != 0 || fy != 0 || fz != 0)
3699 {
3700 //m_taintdisable = true;
3701 //base.RaiseOutOfBounds(Position);
3702 //d.BodySetLinearVel(Body, fx, fy, 0f);
3703 if (!d.BodyIsEnabled(Body))
3704 {
3705 // A physical body at rest on a surface will auto-disable after a while,
3706 // this appears to re-enable it incase the surface it is upon vanishes,
3707 // and the body should fall again.
3708 d.BodySetLinearVel(Body, 0f, 0f, 0f);
3709 d.BodySetForce(Body, 0, 0, 0);
3710 enableBodySoft();
3711 }
3712
3713 // 35x10 = 350n times the mass per second applied maximum.
3714 float nmax = 35f * m_mass;
3715 float nmin = -35f * m_mass;
3716
3717
3718 if (fx > nmax)
3719 fx = nmax;
3720 if (fx < nmin)
3721 fx = nmin;
3722 if (fy > nmax)
3723 fy = nmax;
3724 if (fy < nmin)
3725 fy = nmin;
3726 d.BodyAddForce(Body, fx, fy, fz);
3727//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
3728 }
3729 }
3730 }
3731 else
3732 { // is not physical, or is not a body or is selected
3733 // from old UpdatePositionAndVelocity, ... Not a body.. so Make sure the client isn't interpolating
3734 _velocity.X = 0;
3735 _velocity.Y = 0;
3736 _velocity.Z = 0;
3737
3738 _acceleration.X = 0;
3739 _acceleration.Y = 0;
3740 _acceleration.Z = 0;
3741
3742 m_rotationalVelocity.X = 0;
3743 m_rotationalVelocity.Y = 0;
3744 m_rotationalVelocity.Z = 0;
3745 _zeroFlag = true;
3746 return;
3747 }
3748 } // end Move()
3749 } // end class
3296} 3750}
diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
index 8060ba0..880c3ea 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
@@ -208,8 +208,6 @@ namespace OpenSim.Region.Physics.Manager
208 public abstract void VehicleFloatParam(int param, float value); 208 public abstract void VehicleFloatParam(int param, float value);
209 public abstract void VehicleVectorParam(int param, Vector3 value); 209 public abstract void VehicleVectorParam(int param, Vector3 value);
210 public abstract void VehicleRotationParam(int param, Quaternion rotation); 210 public abstract void VehicleRotationParam(int param, Quaternion rotation);
211 public abstract void VehicleFlagsSet(int flags);
212 public abstract void VehicleFlagsRemove(int flags);
213 public abstract void VehicleFlags(int param, bool remove); 211 public abstract void VehicleFlags(int param, bool remove);
214 212
215 public abstract void SetVolumeDetect(int param); // Allows the detection of collisions with inherently non-physical prims. see llVolumeDetect for more 213 public abstract void SetVolumeDetect(int param); // Allows the detection of collisions with inherently non-physical prims. see llVolumeDetect for more
@@ -354,17 +352,7 @@ namespace OpenSim.Region.Physics.Manager
354 { 352 {
355 353
356 } 354 }
357
358 public override void VehicleFlagsSet(int flags)
359 {
360
361 }
362
363 public override void VehicleFlagsRemove(int flags)
364 {
365 355
366 }
367
368 public override void VehicleFlags(int param, bool remove) 356 public override void VehicleFlags(int param, bool remove)
369 { 357 {
370 358
diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
index 6b8d1e2..2a66060 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
@@ -733,17 +733,7 @@ namespace OpenSim.Region.Physics.OdePlugin
733 { 733 {
734 734
735 } 735 }
736
737 public override void VehicleFlagsSet(int flags)
738 {
739
740 }
741
742 public override void VehicleFlagsRemove(int flags)
743 {
744 736
745 }
746
747 public override void VehicleFlags(int param, bool remove) 737 public override void VehicleFlags(int param, bool remove)
748 { 738 {
749 739
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index 9ef2e9c..2ab00a3 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -2354,16 +2354,6 @@ Console.WriteLine(" JointCreateFixed");
2354 { 2354 {
2355 m_vehicle.ProcessRotationVehicleParam((Vehicle) param, rotation); 2355 m_vehicle.ProcessRotationVehicleParam((Vehicle) param, rotation);
2356 } 2356 }
2357
2358 public override void VehicleFlagsSet(int flags)
2359 {
2360
2361 }
2362
2363 public override void VehicleFlagsRemove(int flags)
2364 {
2365
2366 }
2367 2357
2368 public override void VehicleFlags(int param, bool remove) 2358 public override void VehicleFlags(int param, bool remove)
2369 { 2359 {
diff --git a/OpenSim/Region/Physics/POSPlugin/POSCharacter.cs b/OpenSim/Region/Physics/POSPlugin/POSCharacter.cs
index e722666..a70179b 100644
--- a/OpenSim/Region/Physics/POSPlugin/POSCharacter.cs
+++ b/OpenSim/Region/Physics/POSPlugin/POSCharacter.cs
@@ -181,17 +181,7 @@ namespace OpenSim.Region.Physics.POSPlugin
181 { 181 {
182 182
183 } 183 }
184
185 public override void VehicleFlagsSet(int flags)
186 {
187
188 }
189
190 public override void VehicleFlagsRemove(int flags)
191 {
192 184
193 }
194
195 public override void VehicleFlags(int param, bool remove) { } 185 public override void VehicleFlags(int param, bool remove) { }
196 186
197 public override void SetVolumeDetect(int param) 187 public override void SetVolumeDetect(int param)
diff --git a/OpenSim/Region/Physics/POSPlugin/POSPrim.cs b/OpenSim/Region/Physics/POSPlugin/POSPrim.cs
index c129340..9ba5ebb 100644
--- a/OpenSim/Region/Physics/POSPlugin/POSPrim.cs
+++ b/OpenSim/Region/Physics/POSPlugin/POSPrim.cs
@@ -138,17 +138,7 @@ namespace OpenSim.Region.Physics.POSPlugin
138 { 138 {
139 139
140 } 140 }
141
142 public override void VehicleFlagsSet(int flags)
143 {
144 141
145 }
146
147 public override void VehicleFlagsRemove(int flags)
148 {
149
150 }
151
152 public override void VehicleFlags(int param, bool remove) { } 142 public override void VehicleFlags(int param, bool remove) { }
153 143
154 public override void SetVolumeDetect(int param) 144 public override void SetVolumeDetect(int param)
diff --git a/OpenSim/Region/Physics/PhysXPlugin/PhysXCharacter.cs b/OpenSim/Region/Physics/PhysXPlugin/PhysXCharacter.cs
index fc3adac..92261cd 100644
--- a/OpenSim/Region/Physics/PhysXPlugin/PhysXCharacter.cs
+++ b/OpenSim/Region/Physics/PhysXPlugin/PhysXCharacter.cs
@@ -188,14 +188,6 @@ namespace OpenSim.Region.Physics.PhysXPlugin
188 { 188 {
189 } 189 }
190 190
191 public override void VehicleFlagsSet(int param)
192 {
193 }
194
195 public override void VehicleFlagsRemove(int param)
196 {
197 }
198
199 public override void VehicleFlags(int param, bool remove) 191 public override void VehicleFlags(int param, bool remove)
200 { 192 {
201 } 193 }
diff --git a/OpenSim/Region/Physics/PhysXPlugin/PhysXPrim.cs b/OpenSim/Region/Physics/PhysXPlugin/PhysXPrim.cs
index dc70c80..c0e24fd 100644
--- a/OpenSim/Region/Physics/PhysXPlugin/PhysXPrim.cs
+++ b/OpenSim/Region/Physics/PhysXPlugin/PhysXPrim.cs
@@ -277,8 +277,6 @@ namespace OpenSim.Region.Physics.PhysXPlugin
277 277
278 } 278 }
279 279
280 public override void VehicleFlagsSet(int param) { }
281 public override void VehicleFlagsRemove(int param) { }
282 public override void VehicleFlags(int param, bool remove) { } 280 public override void VehicleFlags(int param, bool remove) { }
283 281
284 public override void SetVolumeDetect(int param) 282 public override void SetVolumeDetect(int param)
generated by cgit v1.1 at 2024-12-29 23:46:12 +0000