aboutsummaryrefslogtreecommitdiffstatshomepage
diff options
context:
space:
mode:
authorKitto Flora2010-03-11 19:12:38 -0500
committerKitto Flora2010-03-11 19:12:38 -0500
commitf6f6ef1532517972b973d8a500818dcd50873352 (patch)
tree3e610efdacb64a63b61d325ee93b9f33ca187ed4
parentFix Physics angular reference frame. (diff)
downloadopensim-SC-f6f6ef1532517972b973d8a500818dcd50873352.zip
opensim-SC-f6f6ef1532517972b973d8a500818dcd50873352.tar.gz
opensim-SC-f6f6ef1532517972b973d8a500818dcd50873352.tar.bz2
opensim-SC-f6f6ef1532517972b973d8a500818dcd50873352.tar.xz
Dynamics Integration Part 1
Diffstat
-rw-r--r--OpenSim/Region/Framework/Scenes/SceneObjectPart.cs11
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs817
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs2572
3 files changed, 1644 insertions, 1756 deletions
diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs
index 0d19589..1353518 100644
--- a/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs
+++ b/OpenSim/Region/Framework/Scenes/SceneObjectPart.cs
@@ -747,7 +747,16 @@ namespace OpenSim.Region.Framework.Scenes
747 /// <summary></summary> 747 /// <summary></summary>
748 public Vector3 Acceleration 748 public Vector3 Acceleration
749 { 749 {
750 get { return m_acceleration; } 750 get
751 {
752 PhysicsActor actor = PhysActor;
753 if (actor != null)
754 {
755 m_acceleration = actor.Acceleration;
756 }
757 return m_acceleration;
758 }
759
751 set { m_acceleration = value; } 760 set { m_acceleration = value; }
752 } 761 }
753 762
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs
deleted file mode 100644
index b3b09e6..0000000
--- a/OpenSim/Region/Physics/ChOdePlugin/ODEDynamics.cs
+++ /dev/null
@@ -1,817 +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 private Vector3 m_angularLock = Vector3.One;
121
122 //Deflection properties
123 // private float m_angularDeflectionEfficiency = 0;
124 // private float m_angularDeflectionTimescale = 0;
125 // private float m_linearDeflectionEfficiency = 0;
126 // private float m_linearDeflectionTimescale = 0;
127
128 //Banking properties
129 // private float m_bankingEfficiency = 0;
130 // private float m_bankingMix = 0;
131 // private float m_bankingTimescale = 0;
132
133 //Hover and Buoyancy properties
134 private float m_VhoverHeight = 0f;
135// private float m_VhoverEfficiency = 0f;
136 private float m_VhoverTimescale = 0f;
137 private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height
138 private float m_VehicleBuoyancy = 0f; // Set by VEHICLE_BUOYANCY, for a vehicle.
139 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
140 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
141 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
142
143 //Attractor properties
144 private float m_verticalAttractionEfficiency = 1.0f; // damped
145 private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor.
146
147
148
149
150
151 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
152 {
153 switch (pParam)
154 {
155 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
156 if (pValue < 0.01f) pValue = 0.01f;
157 // m_angularDeflectionEfficiency = pValue;
158 break;
159 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
160 if (pValue < 0.01f) pValue = 0.01f;
161 // m_angularDeflectionTimescale = pValue;
162 break;
163 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
164 if (pValue < 0.01f) pValue = 0.01f;
165 m_angularMotorDecayTimescale = pValue;
166 break;
167 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
168 if (pValue < 0.01f) pValue = 0.01f;
169 m_angularMotorTimescale = pValue;
170 break;
171 case Vehicle.BANKING_EFFICIENCY:
172 if (pValue < 0.01f) pValue = 0.01f;
173 // m_bankingEfficiency = pValue;
174 break;
175 case Vehicle.BANKING_MIX:
176 if (pValue < 0.01f) pValue = 0.01f;
177 // m_bankingMix = pValue;
178 break;
179 case Vehicle.BANKING_TIMESCALE:
180 if (pValue < 0.01f) pValue = 0.01f;
181 // m_bankingTimescale = pValue;
182 break;
183 case Vehicle.BUOYANCY:
184 if (pValue < -1f) pValue = -1f;
185 if (pValue > 1f) pValue = 1f;
186 m_VehicleBuoyancy = pValue;
187 break;
188// case Vehicle.HOVER_EFFICIENCY:
189// if (pValue < 0f) pValue = 0f;
190// if (pValue > 1f) pValue = 1f;
191// m_VhoverEfficiency = pValue;
192// break;
193 case Vehicle.HOVER_HEIGHT:
194 m_VhoverHeight = pValue;
195 break;
196 case Vehicle.HOVER_TIMESCALE:
197 if (pValue < 0.01f) pValue = 0.01f;
198 m_VhoverTimescale = pValue;
199 break;
200 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
201 if (pValue < 0.01f) pValue = 0.01f;
202 // m_linearDeflectionEfficiency = pValue;
203 break;
204 case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
205 if (pValue < 0.01f) pValue = 0.01f;
206 // m_linearDeflectionTimescale = pValue;
207 break;
208 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
209 if (pValue < 0.01f) pValue = 0.01f;
210 m_linearMotorDecayTimescale = pValue;
211 break;
212 case Vehicle.LINEAR_MOTOR_TIMESCALE:
213 if (pValue < 0.01f) pValue = 0.01f;
214 m_linearMotorTimescale = pValue;
215 break;
216 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
217 if (pValue < 0.1f) pValue = 0.1f; // Less goes unstable
218 if (pValue > 1.0f) pValue = 1.0f;
219 m_verticalAttractionEfficiency = pValue;
220 break;
221 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
222 if (pValue < 0.01f) pValue = 0.01f;
223 m_verticalAttractionTimescale = pValue;
224 break;
225
226 // These are vector properties but the engine lets you use a single float value to
227 // set all of the components to the same value
228 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
229 if (pValue > 30f) pValue = 30f;
230 if (pValue < 0.1f) pValue = 0.1f;
231 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
232 break;
233 case Vehicle.ANGULAR_MOTOR_DIRECTION:
234 m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
235 UpdateAngDecay();
236 break;
237 case Vehicle.LINEAR_FRICTION_TIMESCALE:
238 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
239 break;
240 case Vehicle.LINEAR_MOTOR_DIRECTION:
241 m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
242 UpdateLinDecay();
243 break;
244 case Vehicle.LINEAR_MOTOR_OFFSET:
245 // m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
246 break;
247
248 }
249
250 }//end ProcessFloatVehicleParam
251
252 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
253 {
254 switch (pParam)
255 {
256 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
257 if (pValue.X > 30f) pValue.X = 30f;
258 if (pValue.X < 0.1f) pValue.X = 0.1f;
259 if (pValue.Y > 30f) pValue.Y = 30f;
260 if (pValue.Y < 0.1f) pValue.Y = 0.1f;
261 if (pValue.Z > 30f) pValue.Z = 30f;
262 if (pValue.Z < 0.1f) pValue.Z = 0.1f;
263 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
264 break;
265 case Vehicle.ANGULAR_MOTOR_DIRECTION:
266 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
267 // Limit requested angular speed to 2 rps= 4 pi rads/sec
268 if(m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f;
269 if(m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f;
270 if(m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f;
271 if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f;
272 if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f;
273 if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f;
274 UpdateAngDecay();
275 break;
276 case Vehicle.LINEAR_FRICTION_TIMESCALE:
277 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
278 break;
279 case Vehicle.LINEAR_MOTOR_DIRECTION:
280 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); // velocity requested by LSL, for max limiting
281 UpdateLinDecay();
282 break;
283 case Vehicle.LINEAR_MOTOR_OFFSET:
284 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
285 break;
286 }
287
288 }//end ProcessVectorVehicleParam
289
290 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
291 {
292 switch (pParam)
293 {
294 case Vehicle.REFERENCE_FRAME:
295 // m_referenceFrame = pValue;
296 break;
297 }
298
299 }//end ProcessRotationVehicleParam
300
301 internal void SetAngularLock(Vector3 pValue)
302 {
303 m_angularLock = pValue;
304 }
305
306 internal void ProcessFlagsVehicleSet(int flags)
307 {
308 m_flags |= (VehicleFlag)flags;
309 }
310
311 internal void ProcessFlagsVehicleRemove(int flags)
312 {
313 m_flags &= ~((VehicleFlag)flags);
314 }
315
316 internal void ProcessTypeChange(Vehicle pType)
317 {
318 // Set Defaults For Type
319 m_type = pType;
320 switch (pType)
321 {
322 case Vehicle.TYPE_SLED:
323 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
324 m_angularFrictionTimescale = new Vector3(30, 30, 30);
325// m_lLinMotorVel = Vector3.Zero;
326 m_linearMotorTimescale = 1000;
327 m_linearMotorDecayTimescale = 120;
328 m_angularMotorDirection = Vector3.Zero;
329 m_angularMotorDVel = Vector3.Zero;
330 m_angularMotorTimescale = 1000;
331 m_angularMotorDecayTimescale = 120;
332 m_VhoverHeight = 0;
333// m_VhoverEfficiency = 1;
334 m_VhoverTimescale = 10;
335 m_VehicleBuoyancy = 0;
336 // m_linearDeflectionEfficiency = 1;
337 // m_linearDeflectionTimescale = 1;
338 // m_angularDeflectionEfficiency = 1;
339 // m_angularDeflectionTimescale = 1000;
340 // m_bankingEfficiency = 0;
341 // m_bankingMix = 1;
342 // m_bankingTimescale = 10;
343 // m_referenceFrame = Quaternion.Identity;
344 m_flags &=
345 ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
346 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
347 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
348 break;
349 case Vehicle.TYPE_CAR:
350 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
351 m_angularFrictionTimescale = new Vector3(30, 30, 30); // was 1000, but sl max frict time is 30.
352// m_lLinMotorVel = Vector3.Zero;
353 m_linearMotorTimescale = 1;
354 m_linearMotorDecayTimescale = 60;
355 m_angularMotorDirection = Vector3.Zero;
356 m_angularMotorDVel = Vector3.Zero;
357 m_angularMotorTimescale = 1;
358 m_angularMotorDecayTimescale = 0.8f;
359 m_VhoverHeight = 0;
360// m_VhoverEfficiency = 0;
361 m_VhoverTimescale = 1000;
362 m_VehicleBuoyancy = 0;
363 // // m_linearDeflectionEfficiency = 1;
364 // // m_linearDeflectionTimescale = 2;
365 // // m_angularDeflectionEfficiency = 0;
366 // m_angularDeflectionTimescale = 10;
367 m_verticalAttractionEfficiency = 1f;
368 m_verticalAttractionTimescale = 10f;
369 // m_bankingEfficiency = -0.2f;
370 // m_bankingMix = 1;
371 // m_bankingTimescale = 1;
372 // m_referenceFrame = Quaternion.Identity;
373 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
374 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY |
375 VehicleFlag.LIMIT_MOTOR_UP);
376 break;
377 case Vehicle.TYPE_BOAT:
378 m_linearFrictionTimescale = new Vector3(10, 3, 2);
379 m_angularFrictionTimescale = new Vector3(10,10,10);
380// m_lLinMotorVel = Vector3.Zero;
381 m_linearMotorTimescale = 5;
382 m_linearMotorDecayTimescale = 60;
383 m_angularMotorDirection = Vector3.Zero;
384 m_angularMotorDVel = Vector3.Zero;
385 m_angularMotorTimescale = 4;
386 m_angularMotorDecayTimescale = 4;
387 m_VhoverHeight = 0;
388// m_VhoverEfficiency = 0.5f;
389 m_VhoverTimescale = 2;
390 m_VehicleBuoyancy = 1;
391 // m_linearDeflectionEfficiency = 0.5f;
392 // m_linearDeflectionTimescale = 3;
393 // m_angularDeflectionEfficiency = 0.5f;
394 // m_angularDeflectionTimescale = 5;
395 m_verticalAttractionEfficiency = 0.5f;
396 m_verticalAttractionTimescale = 5f;
397 // m_bankingEfficiency = -0.3f;
398 // m_bankingMix = 0.8f;
399 // m_bankingTimescale = 1;
400 // m_referenceFrame = Quaternion.Identity;
401 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY |
402 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
403 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY |
404 VehicleFlag.LIMIT_MOTOR_UP);
405 break;
406 case Vehicle.TYPE_AIRPLANE:
407 m_linearFrictionTimescale = new Vector3(200, 10, 5);
408 m_angularFrictionTimescale = new Vector3(20, 20, 20);
409// m_lLinMotorVel = Vector3.Zero;
410 m_linearMotorTimescale = 2;
411 m_linearMotorDecayTimescale = 60;
412 m_angularMotorDirection = Vector3.Zero;
413 m_angularMotorDVel = Vector3.Zero;
414 m_angularMotorTimescale = 4;
415 m_angularMotorDecayTimescale = 4;
416 m_VhoverHeight = 0;
417// m_VhoverEfficiency = 0.5f;
418 m_VhoverTimescale = 1000;
419 m_VehicleBuoyancy = 0;
420 // m_linearDeflectionEfficiency = 0.5f;
421 // m_linearDeflectionTimescale = 3;
422 // m_angularDeflectionEfficiency = 1;
423 // m_angularDeflectionTimescale = 2;
424 m_verticalAttractionEfficiency = 0.9f;
425 m_verticalAttractionTimescale = 2f;
426 // m_bankingEfficiency = 1;
427 // m_bankingMix = 0.7f;
428 // m_bankingTimescale = 2;
429 // m_referenceFrame = Quaternion.Identity;
430 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
431 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
432 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
433 break;
434 case Vehicle.TYPE_BALLOON:
435 m_linearFrictionTimescale = new Vector3(5, 5, 5);
436 m_angularFrictionTimescale = new Vector3(10, 10, 10);
437 m_linearMotorTimescale = 5;
438 m_linearMotorDecayTimescale = 60;
439 m_angularMotorDirection = Vector3.Zero;
440 m_angularMotorDVel = Vector3.Zero;
441 m_angularMotorTimescale = 6;
442 m_angularMotorDecayTimescale = 10;
443 m_VhoverHeight = 5;
444// m_VhoverEfficiency = 0.8f;
445 m_VhoverTimescale = 10;
446 m_VehicleBuoyancy = 1;
447 // m_linearDeflectionEfficiency = 0;
448 // m_linearDeflectionTimescale = 5;
449 // m_angularDeflectionEfficiency = 0;
450 // m_angularDeflectionTimescale = 5;
451 m_verticalAttractionEfficiency = 1f;
452 m_verticalAttractionTimescale = 100f;
453 // m_bankingEfficiency = 0;
454 // m_bankingMix = 0.7f;
455 // m_bankingTimescale = 5;
456 // m_referenceFrame = Quaternion.Identity;
457 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
458 VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
459 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
460 break;
461
462 }
463 }//end SetDefaultsForType
464
465 internal void Enable(IntPtr pBody, OdeScene pParentScene)
466 {
467 if (m_type == Vehicle.TYPE_NONE)
468 return;
469
470 m_body = pBody;
471 }
472
473 internal void Step(float pTimestep, OdeScene pParentScene)
474 {
475 if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
476 return;
477 frcount++; // used to limit debug comment output
478 if (frcount > 24)
479 frcount = 0;
480
481 MoveLinear(pTimestep, pParentScene);
482 MoveAngular(pTimestep);
483 }// end Step
484
485 internal void Halt()
486 { // Kill all motions, when non-physical
487 m_linearMotorDirection = Vector3.Zero;
488 m_lLinMotorDVel = Vector3.Zero;
489 m_lLinObjectVel = Vector3.Zero;
490 m_wLinObjectVel = Vector3.Zero;
491 m_angularMotorDirection = Vector3.Zero;
492 m_lastAngularVelocity = Vector3.Zero;
493 m_angularMotorDVel = Vector3.Zero;
494 }
495
496 private void UpdateLinDecay()
497 {
498 if (Math.Abs(m_linearMotorDirection.X) > Math.Abs(m_lLinMotorDVel.X)) m_lLinMotorDVel.X = m_linearMotorDirection.X;
499 if (Math.Abs(m_linearMotorDirection.Y) > Math.Abs(m_lLinMotorDVel.Y)) m_lLinMotorDVel.Y = m_linearMotorDirection.Y;
500 if (Math.Abs(m_linearMotorDirection.Z) > Math.Abs(m_lLinMotorDVel.Z)) m_lLinMotorDVel.Z = m_linearMotorDirection.Z;
501 } // else let the motor decay on its own
502
503 private void MoveLinear(float pTimestep, OdeScene _pParentScene)
504 {
505 Vector3 acceleration = new Vector3(0f, 0f, 0f);
506
507 d.Quaternion rot = d.BodyGetQuaternion(Body);
508 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
509 Quaternion irotq = Quaternion.Inverse(rotq);
510 d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame
511 Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z);
512 acceleration = vel_now - m_wLinObjectVel;
513 m_lLinObjectVel = vel_now * irotq;
514
515 if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate
516 {
517 if ( Vector3.Mag(m_lLinMotorDVel) < 1.0f)
518 {
519 float decayfactor = m_linearMotorDecayTimescale/pTimestep;
520 Vector3 decayAmount = (m_lLinMotorDVel/decayfactor);
521 m_lLinMotorDVel -= decayAmount;
522 }
523 else
524 {
525 float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale)));
526 Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * pTimestep;
527 m_lLinMotorDVel -= decel;
528 }
529 if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
530 {
531 m_lLinMotorDVel = Vector3.Zero;
532 }
533 else
534 {
535 if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X;
536 if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y;
537 if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z;
538 }
539 }
540
541 if ( (! m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) )
542 {
543 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
544 if (m_linearMotorTimescale < 300.0f)
545 {
546 Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel;
547 float linfactor = m_linearMotorTimescale/pTimestep;
548 Vector3 attackAmount = (attack_error/linfactor) * 1.3f;
549 m_lLinObjectVel += attackAmount;
550 }
551 if (m_linearFrictionTimescale.X < 300.0f)
552 {
553 float fricfactor = m_linearFrictionTimescale.X / pTimestep;
554 float fricX = m_lLinObjectVel.X / fricfactor;
555 m_lLinObjectVel.X -= fricX;
556 }
557 if (m_linearFrictionTimescale.Y < 300.0f)
558 {
559 float fricfactor = m_linearFrictionTimescale.Y / pTimestep;
560 float fricY = m_lLinObjectVel.Y / fricfactor;
561 m_lLinObjectVel.Y -= fricY;
562 }
563 if (m_linearFrictionTimescale.Z < 300.0f)
564 {
565 float fricfactor = m_linearFrictionTimescale.Z / pTimestep;
566//if(frcount == 0) Console.WriteLine("Zfric={0}", fricfactor);
567 float fricZ = m_lLinObjectVel.Z / fricfactor;
568 m_lLinObjectVel.Z -= fricZ;
569 }
570 }
571 m_wLinObjectVel = m_lLinObjectVel * rotq;
572 // Add Gravity and Buoyancy
573 Vector3 grav = Vector3.Zero;
574 if(m_VehicleBuoyancy < 1.0f)
575 {
576 // There is some gravity, make a gravity force vector
577 // that is applied after object velocity.
578 d.Mass objMass;
579 d.BodyGetMass(Body, out objMass);
580 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
581 grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force
582 } // else its 1.0, no gravity.
583
584 // Check if hovering
585 if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
586 {
587 // We should hover, get the target height
588 d.Vector3 pos = d.BodyGetPosition(Body);
589 if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY)
590 {
591 m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight;
592 }
593 else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY)
594 {
595 m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
596 }
597 else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT)
598 {
599 m_VhoverTargetHeight = m_VhoverHeight;
600 }
601
602 if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY)
603 {
604 // If body is aready heigher, use its height as target height
605 if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
606 }
607
608// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
609// m_VhoverTimescale = 0f; // time to acheive height
610// pTimestep is time since last frame,in secs
611 float herr0 = pos.Z - m_VhoverTargetHeight;
612 // Replace Vertical speed with correction figure if significant
613 if(Math.Abs(herr0) > 0.01f )
614 {
615 d.Mass objMass;
616 d.BodyGetMass(Body, out objMass);
617 m_wLinObjectVel.Z = - ( (herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
618 //KF: m_VhoverEfficiency is not yet implemented
619 }
620 else
621 {
622 m_wLinObjectVel.Z = 0f;
623 }
624 }
625 else
626 { // not hovering, Gravity rules
627 m_wLinObjectVel.Z = vel_now.Z;
628//if(frcount == 0) Console.WriteLine(" Z {0} a.Z {1}", m_wLinObjectVel.Z, acceleration.Z);
629 }
630 // Apply velocity
631 d.BodySetLinearVel(Body, m_wLinObjectVel.X, m_wLinObjectVel.Y, m_wLinObjectVel.Z);
632 // apply gravity force
633 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
634//if(frcount == 0) Console.WriteLine("Grav {0}", grav);
635 } // end MoveLinear()
636
637 private void UpdateAngDecay()
638 {
639 if (Math.Abs(m_angularMotorDirection.X) > Math.Abs(m_angularMotorDVel.X)) m_angularMotorDVel.X = m_angularMotorDirection.X;
640 if (Math.Abs(m_angularMotorDirection.Y) > Math.Abs(m_angularMotorDVel.Y)) m_angularMotorDVel.Y = m_angularMotorDirection.Y;
641 if (Math.Abs(m_angularMotorDirection.Z) > Math.Abs(m_angularMotorDVel.Z)) m_angularMotorDVel.Z = m_angularMotorDirection.Z;
642 } // else let the motor decay on its own
643
644 private void MoveAngular(float pTimestep)
645 {
646 /*
647 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
648
649 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
650 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
651 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
652
653 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
654 private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body
655 */
656//if(frcount == 0) Console.WriteLine("MoveAngular ");
657
658 // Get what the body is doing, this includes 'external' influences
659 d.Quaternion rot = d.BodyGetQuaternion(Body);
660 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
661 Quaternion irotq = Quaternion.Inverse(rotq);
662 d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body);
663 Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z);
664 angObjectVel = angObjectVel * irotq; // ============ Converts to LOCAL rotation
665
666//if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel);
667// Vector3 FrAaccel = m_lastAngularVelocity - angObjectVel;
668// Vector3 initavel = angObjectVel;
669 // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack.
670 float atk_decayfactor = 23.0f / (m_angularMotorTimescale * pTimestep);
671 m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor;
672 // Decay Angular Motor 2.
673 if (m_angularMotorDecayTimescale < 300.0f)
674 {
675//####
676 if ( Vector3.Mag(m_angularMotorDVel) < 1.0f)
677 {
678 float decayfactor = (m_angularMotorDecayTimescale)/pTimestep;
679 Vector3 decayAmount = (m_angularMotorDVel/decayfactor);
680 m_angularMotorDVel -= decayAmount;
681 }
682 else
683 {
684 Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * pTimestep / m_angularMotorDecayTimescale;
685 m_angularMotorDVel -= decel;
686 }
687
688 if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
689 {
690 m_angularMotorDVel = Vector3.Zero;
691 }
692 else
693 {
694 if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X;
695 if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y;
696 if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z;
697 }
698 } // end decay angular motor
699//if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel);
700
701//if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel);
702 // Vertical attractor section
703 Vector3 vertattr = Vector3.Zero;
704
705 if(m_verticalAttractionTimescale < 300)
706 {
707 float VAservo = 1.0f / (m_verticalAttractionTimescale * pTimestep);
708 // get present body rotation
709// d.Quaternion rot = d.BodyGetQuaternion(Body);
710// Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
711 // make a vector pointing up
712 Vector3 verterr = Vector3.Zero;
713 verterr.Z = 1.0f;
714 // rotate it to Body Angle
715 verterr = verterr * rotq;
716 // 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.
717 // 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
718 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
719
720 if (verterr.Z < 0.0f)
721 { // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to
722 // vertical, BUT for some reason a z-rotation is imparted to the object. TBI.
723//Console.WriteLine("InvertFlip");
724 verterr.X = 2.0f - verterr.X;
725 verterr.Y = 2.0f - verterr.Y;
726 }
727 verterr *= 0.5f;
728 // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt)
729
730 if ((!angObjectVel.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f))
731 {
732//if(frcount == 0)
733 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
734 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
735 vertattr.X = verterr.Y;
736 vertattr.Y = - verterr.X;
737 vertattr.Z = 0f;
738//if(frcount == 0) Console.WriteLine("VAerr=" + verterr);
739
740 // scaling appears better usingsquare-law
741 float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
742 float bounce = 1.0f - damped;
743 // 0 = crit damp, 1 = bouncy
744 float oavz = angObjectVel.Z; // retain z velocity
745 angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce; // The time-scaled correction, which sums, therefore is bouncy
746 angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped); // damped, good @ < 90.
747 angObjectVel.Z = oavz;
748//if(frcount == 0) Console.WriteLine("VA+");
749//Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel);
750 }
751 else
752 {
753 // else error is very small
754 angObjectVel.X = 0f;
755 angObjectVel.Y = 0f;
756//if(frcount == 0) Console.WriteLine("VA0");
757 }
758 } // else vertical attractor is off
759//if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel);
760
761 if ( (! m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) )
762 { // if motor or object have motion
763 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
764
765 if (m_angularMotorTimescale < 300.0f)
766 {
767 Vector3 attack_error = m_angularMotorDVel - angObjectVel;
768 float angfactor = m_angularMotorTimescale/pTimestep;
769 Vector3 attackAmount = (attack_error/angfactor);
770 angObjectVel += attackAmount;
771//if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount);
772//if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel);
773 }
774
775 angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / pTimestep);
776 angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / pTimestep);
777 angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / pTimestep);
778 } // else no signif. motion
779
780//if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel);
781 // Bank section tba
782 // Deflection section tba
783//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel);
784
785 m_lastAngularVelocity = angObjectVel;
786/*
787 if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.0001f))
788 {
789 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
790 }
791 else
792 {
793 m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
794 }
795 */
796//if(frcount == 0) Console.WriteLine("angularLock {0}", m_angularLock);
797
798 if (!m_angularLock.ApproxEquals(Vector3.One, 0.003f))
799 {
800 if (m_angularLock.X == 0)
801 m_lastAngularVelocity.X = 0f;
802 if (m_angularLock.Y == 0)
803 m_lastAngularVelocity.Y = 0f;
804 if (m_angularLock.Z == 0)
805 m_lastAngularVelocity.Z = 0f;
806 }
807 // Apply to the body
808// Vector3 aInc = m_lastAngularVelocity - initavel;
809//if(frcount == 0) Console.WriteLine("Inc {0}", aInc);
810 m_lastAngularVelocity = m_lastAngularVelocity * rotq; // ================ Converts to WORLD rotation
811
812 d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
813//if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity);
814
815 } //end MoveAngular
816 }
817}
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs
index fb6cb55..5d24388 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>
@@ -102,8 +81,8 @@ namespace OpenSim.Region.Physics.OdePlugin
102 private float m_APIDDamping = 0.5f; 81 private float m_APIDDamping = 0.5f;
103 private bool m_useAPID = false; 82 private bool m_useAPID = false;
104 83
105 // 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),
106 // and are for non-VEHICLES only. 85 // do not confuse with VEHICLE HOVER
107 86
108 private float m_PIDHoverHeight; 87 private float m_PIDHoverHeight;
109 private float m_PIDHoverTau; 88 private float m_PIDHoverTau;
@@ -112,7 +91,7 @@ namespace OpenSim.Region.Physics.OdePlugin
112 private float m_targetHoverHeight; 91 private float m_targetHoverHeight;
113 private float m_groundHeight; 92 private float m_groundHeight;
114 private float m_waterHeight; 93 private float m_waterHeight;
115 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()
116 95
117 // private float m_tensor = 5f; 96 // private float m_tensor = 5f;
118 private int body_autodisable_frames = 20; 97 private int body_autodisable_frames = 20;
@@ -183,7 +162,7 @@ namespace OpenSim.Region.Physics.OdePlugin
183 public bool outofBounds; 162 public bool outofBounds;
184 private float m_density = 10.000006836f; // Aluminum g/cm3; 163 private float m_density = 10.000006836f; // Aluminum g/cm3;
185 164
186 public bool _zeroFlag; 165 public bool _zeroFlag; // if body has been stopped
187 private bool m_lastUpdateSent; 166 private bool m_lastUpdateSent;
188 167
189 public IntPtr Body = IntPtr.Zero; 168 public IntPtr Body = IntPtr.Zero;
@@ -198,18 +177,81 @@ namespace OpenSim.Region.Physics.OdePlugin
198 177
199 public volatile bool childPrim; 178 public volatile bool childPrim;
200 179
201 private ODEDynamics m_vehicle;
202
203 internal int m_material = (int)Material.Wood; 180 internal int m_material = (int)Material.Wood;
204 181
205 private int frcount = 0; // Used to limit dynamics debug output to 182 private int frcount = 0; // Used to limit dynamics debug output to
206 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 private Vector3 m_angularLock = Vector3.One;
221
222 //Deflection properties
223 // private float m_angularDeflectionEfficiency = 0;
224 // private float m_angularDeflectionTimescale = 0;
225 // private float m_linearDeflectionEfficiency = 0;
226 // private float m_linearDeflectionTimescale = 0;
227
228 //Banking properties
229 // private float m_bankingEfficiency = 0;
230 // private float m_bankingMix = 0;
231 // private float m_bankingTimescale = 0;
232
233 //Hover and Buoyancy properties
234 private float m_VhoverHeight = 0f;
235// private float m_VhoverEfficiency = 0f;
236 private float m_VhoverTimescale = 0f;
237 private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height
238 private float m_VehicleBuoyancy = 0f; // Set by VEHICLE_BUOYANCY, for a vehicle.
239 // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
240 // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
241 // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
242
243 //Attractor properties
244 private float m_verticalAttractionEfficiency = 1.0f; // damped
245 private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor.
246
247
248
249
250
207 251
208 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, 252 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
209 Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode) 253 Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, CollisionLocker dode)
210 { 254 {
211 m_vehicle = new ODEDynamics();
212 //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
213 ode = dode; 255 ode = dode;
214 if (!pos.IsFinite()) 256 if (!pos.IsFinite())
215 { 257 {
@@ -302,7 +344,7 @@ namespace OpenSim.Region.Physics.OdePlugin
302 { 344 {
303 set { 345 set {
304 346
305 347Console.WriteLine("Sel {0} {1} {2}", m_primName, value, m_isphysical);
306 // This only makes the object not collidable if the object 348 // This only makes the object not collidable if the object
307 // is physical or the object is modified somehow *IN THE FUTURE* 349 // is physical or the object is modified somehow *IN THE FUTURE*
308 // without this, if an avatar selects prim, they can walk right 350 // without this, if an avatar selects prim, they can walk right
@@ -322,6 +364,416 @@ namespace OpenSim.Region.Physics.OdePlugin
322 } 364 }
323 } 365 }
324 366
367 public override bool IsPhysical
368 {
369 get { return m_isphysical; }
370 set
371 {
372 m_isphysical = value;
373 if (!m_isphysical)
374 { // Zero the remembered last velocity
375 m_lastVelocity = Vector3.Zero;
376 if (m_type != Vehicle.TYPE_NONE) Halt();
377 }
378 }
379 }
380
381 public void setPrimForRemoval()
382 {
383 m_taintremove = true;
384 }
385
386 public override bool Flying
387 {
388 // no flying prims for you
389 get { return false; }
390 set { }
391 }
392
393 public override bool IsColliding
394 {
395 get { return iscolliding; }
396 set { iscolliding = value; }
397 }
398
399 public override bool CollidingGround
400 {
401 get { return false; }
402 set { return; }
403 }
404
405 public override bool CollidingObj
406 {
407 get { return false; }
408 set { return; }
409 }
410
411 public override bool ThrottleUpdates
412 {
413 get { return m_throttleUpdates; }
414 set { m_throttleUpdates = value; }
415 }
416
417 public override bool Stopped
418 {
419 get { return _zeroFlag; }
420 }
421
422 public override Vector3 Position
423 {
424 get { return _position; }
425
426 set { _position = value;
427 //m_log.Info("[PHYSICS]: " + _position.ToString());
428 }
429 }
430
431 public override Vector3 Size
432 {
433 get { return _size; }
434 set
435 {
436 if (value.IsFinite())
437 {
438 _size = value;
439 }
440 else
441 {
442 m_log.Warn("[PHYSICS]: Got NaN Size on object");
443 }
444 }
445 }
446
447 public override float Mass
448 {
449 get { return CalculateMass(); }
450 }
451
452 public override Vector3 Force
453 {
454 //get { return Vector3.Zero; }
455 get { return m_force; }
456 set
457 {
458 if (value.IsFinite())
459 {
460 m_force = value;
461 }
462 else
463 {
464 m_log.Warn("[PHYSICS]: NaN in Force Applied to an Object");
465 }
466 }
467 }
468
469 public override int VehicleType
470 {
471 get { return (int)m_type; }
472 set { ProcessTypeChange((Vehicle)value); }
473 }
474
475 public override void VehicleFloatParam(int param, float value)
476 {
477 ProcessFloatVehicleParam((Vehicle) param, value);
478 }
479
480 public override void VehicleVectorParam(int param, Vector3 value)
481 {
482 ProcessVectorVehicleParam((Vehicle) param, value);
483 }
484
485 public override void VehicleRotationParam(int param, Quaternion rotation)
486 {
487 ProcessRotationVehicleParam((Vehicle) param, rotation);
488 }
489
490 public override void VehicleFlagsSet(int flags)
491 {
492 ProcessFlagsVehicleSet(flags);
493 }
494
495 public override void VehicleFlagsRemove(int flags)
496 {
497 ProcessFlagsVehicleRemove(flags);
498 }
499
500 public override void SetVolumeDetect(int param)
501 {
502 lock (_parent_scene.OdeLock)
503 {
504 m_isVolumeDetect = (param!=0);
505 }
506 }
507
508 public override Vector3 CenterOfMass
509 {
510 get { return Vector3.Zero; }
511 }
512
513 public override Vector3 GeometricCenter
514 {
515 get { return Vector3.Zero; }
516 }
517
518 public override PrimitiveBaseShape Shape
519 {
520 set
521 {
522 _pbs = value;
523 m_taintshape = true;
524 }
525 }
526
527 public override Vector3 Velocity
528 {
529 get
530 {
531 // Averate previous velocity with the new one so
532 // client object interpolation works a 'little' better
533 if (_zeroFlag)
534 return Vector3.Zero;
535
536 Vector3 returnVelocity = Vector3.Zero;
537 returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2;
538 returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2;
539 returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2;
540 return returnVelocity;
541 }
542 set
543 {
544 if (value.IsFinite())
545 {
546 _velocity = value;
547
548 m_taintVelocity = value;
549 _parent_scene.AddPhysicsActorTaint(this);
550 }
551 else
552 {
553 m_log.Warn("[PHYSICS]: Got NaN Velocity in Object");
554 }
555
556 }
557 }
558
559 public override Vector3 Torque
560 {
561 get
562 {
563 if (!m_isphysical || Body == IntPtr.Zero)
564 return Vector3.Zero;
565
566 return _torque;
567 }
568
569 set
570 {
571 if (value.IsFinite())
572 {
573 m_taintTorque = value;
574 _parent_scene.AddPhysicsActorTaint(this);
575 }
576 else
577 {
578 m_log.Warn("[PHYSICS]: Got NaN Torque in Object");
579 }
580 }
581 }
582
583 public override float CollisionScore
584 {
585 get { return m_collisionscore; }
586 set { m_collisionscore = value; }
587 }
588
589 public override bool Kinematic
590 {
591 get { return false; }
592 set { }
593 }
594
595 public override Quaternion Orientation
596 {
597 get { return _orientation; }
598 set
599 {
600 if (QuaternionIsFinite(value))
601 {
602 _orientation = value;
603 }
604 else
605 m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object");
606
607 }
608 }
609
610
611 public override bool FloatOnWater
612 {
613 set {
614 m_taintCollidesWater = value;
615 _parent_scene.AddPhysicsActorTaint(this);
616 }
617 }
618
619 public override void SetMomentum(Vector3 momentum)
620 {
621 }
622
623 public override Vector3 PIDTarget
624 {
625 set
626 {
627 if (value.IsFinite())
628 {
629 m_PIDTarget = value;
630 }
631 else
632 m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object");
633 }
634 }
635 public override bool PIDActive { set { m_usePID = value; } }
636 public override float PIDTau { set { m_PIDTau = value; } }
637
638 // For RotLookAt
639 public override Quaternion APIDTarget { set { m_APIDTarget = value; } }
640 public override bool APIDActive { set { m_useAPID = value; } }
641 public override float APIDStrength { set { m_APIDStrength = value; } }
642 public override float APIDDamping { set { m_APIDDamping = value; } }
643
644 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
645 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
646 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
647 public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
648
649 internal static bool QuaternionIsFinite(Quaternion q)
650 {
651 if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
652 return false;
653 if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
654 return false;
655 if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
656 return false;
657 if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
658 return false;
659 return true;
660 }
661
662 public override Vector3 Acceleration
663 {
664 get { return _acceleration; }
665 }
666
667
668 public void SetAcceleration(Vector3 accel) // No one calls this, and it would not do anything.
669 {
670 _acceleration = accel;
671 }
672
673 public override void AddForce(Vector3 force, bool pushforce)
674 {
675 if (force.IsFinite())
676 {
677 lock (m_forcelist)
678 m_forcelist.Add(force);
679
680 m_taintforce = true;
681 }
682 else
683 {
684 m_log.Warn("[PHYSICS]: Got Invalid linear force vector from Scene in Object");
685 }
686 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
687 }
688
689 public override void AddAngularForce(Vector3 force, bool pushforce)
690 {
691 if (force.IsFinite())
692 {
693 m_angularforcelist.Add(force);
694 m_taintaddangularforce = true;
695 }
696 else
697 {
698 m_log.Warn("[PHYSICS]: Got Invalid Angular force vector from Scene in Object");
699 }
700 }
701
702 public override Vector3 RotationalVelocity
703 {
704 get
705 {
706/* Vector3 pv = Vector3.Zero;
707 if (_zeroFlag)
708 return pv;
709 m_lastUpdateSent = false;
710
711 if (m_rotationalVelocity.ApproxEquals(pv, 0.2f))
712 return pv;
713*/
714 return m_rotationalVelocity;
715 }
716 set
717 {
718 if (value.IsFinite())
719 {
720 m_rotationalVelocity = value;
721 }
722 else
723 {
724 m_log.Warn("[PHYSICS]: Got NaN RotationalVelocity in Object");
725 }
726 }
727 }
728
729 public override void CrossingFailure()
730 {
731 m_crossingfailures++;
732 if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
733 {
734 base.RaiseOutOfBounds(_position);
735 return;
736 }
737 else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
738 {
739 m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName);
740 }
741 }
742
743 public override float Buoyancy
744 {
745 get { return m_buoyancy; }
746 set { m_buoyancy = value; }
747 }
748
749 public override void link(PhysicsActor obj)
750 {
751 m_taintparent = obj;
752 }
753
754 public override void delink()
755 {
756 m_taintparent = null;
757 }
758
759 public override void LockAngularMotion(Vector3 axis)
760 {
761 // reverse the zero/non zero values for ODE.
762 if (axis.IsFinite())
763 {
764 axis.X = (axis.X > 0) ? 1f : 0f;
765 axis.Y = (axis.Y > 0) ? 1f : 0f;
766 axis.Z = (axis.Z > 0) ? 1f : 0f;
767 m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
768 m_taintAngularLock = axis;
769 }
770 else
771 {
772 m_log.Warn("[PHYSICS]: Got NaN locking axis from Scene on Object");
773 }
774 }
775
776
325 public void SetGeom(IntPtr geom) 777 public void SetGeom(IntPtr geom)
326 { 778 {
327 prev_geom = prim_geom; 779 prev_geom = prim_geom;
@@ -345,8 +797,6 @@ namespace OpenSim.Region.Physics.OdePlugin
345 //m_log.Warn("Setting Geom to: " + prim_geom); 797 //m_log.Warn("Setting Geom to: " + prim_geom);
346 } 798 }
347 799
348
349
350 public void enableBodySoft() 800 public void enableBodySoft()
351 { 801 {
352 if (!childPrim) 802 if (!childPrim)
@@ -354,8 +804,8 @@ namespace OpenSim.Region.Physics.OdePlugin
354 if (m_isphysical && Body != IntPtr.Zero) 804 if (m_isphysical && Body != IntPtr.Zero)
355 { 805 {
356 d.BodyEnable(Body); 806 d.BodyEnable(Body);
357 if (m_vehicle.Type != Vehicle.TYPE_NONE) 807 if (m_type != Vehicle.TYPE_NONE)
358 m_vehicle.Enable(Body, _parent_scene); 808 Enable(Body, _parent_scene);
359 } 809 }
360 810
361 m_disabled = false; 811 m_disabled = false;
@@ -406,14 +856,9 @@ namespace OpenSim.Region.Physics.OdePlugin
406 m_collisionscore = 0; 856 m_collisionscore = 0;
407 m_disabled = false; 857 m_disabled = false;
408 858
409 // The body doesn't already have a finite rotation mode set here 859 if (m_type != Vehicle.TYPE_NONE)
410 /* ### if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0.0f)) && _parent == null)
411 {
412 createAMotor(m_angularlock);
413 } */
414 if (m_vehicle.Type != Vehicle.TYPE_NONE)
415 { 860 {
416 m_vehicle.Enable(Body, _parent_scene); 861 Enable(Body, _parent_scene);
417 } 862 }
418 863
419 _parent_scene.addActivePrim(this); 864 _parent_scene.addActivePrim(this);
@@ -894,9 +1339,8 @@ namespace OpenSim.Region.Physics.OdePlugin
894 // } 1339 // }
895 } 1340 }
896 1341
897 public void ProcessTaints(float timestep) 1342 public void ProcessTaints(float timestep) //=============================================================================
898 { 1343 {
899//Console.WriteLine("ProcessTaints for " + m_primName );
900 if (m_taintadd) 1344 if (m_taintadd)
901 { 1345 {
902 changeadd(timestep); 1346 changeadd(timestep);
@@ -985,30 +1429,6 @@ namespace OpenSim.Region.Physics.OdePlugin
985 { 1429 {
986//Console.WriteLine("Alock changed to {0}", m_taintAngularLock); 1430//Console.WriteLine("Alock changed to {0}", m_taintAngularLock);
987 m_angularlock = m_taintAngularLock; 1431 m_angularlock = m_taintAngularLock;
988 m_vehicle.SetAngularLock(m_angularlock);
989
990
991/*
992 if (!m_taintAngularLock.ApproxEquals(Vector3.One, 0f))
993 {
994 //d.BodySetFiniteRotationMode(Body, 0);
995 //d.BodySetFiniteRotationAxis(Body,m_taintAngularLock.X,m_taintAngularLock.Y,m_taintAngularLock.Z);
996 createAMotor(m_taintAngularLock);
997 }
998 else
999 {
1000 if (Amotor != IntPtr.Zero)
1001 {
1002 d.JointDestroy(Amotor);
1003 Amotor = IntPtr.Zero;
1004 }
1005 }
1006 }
1007 }
1008 // Store this for later in case we get turned into a separate body
1009 m_angularlock = m_taintAngularLock;
1010 m_vehicle.SetAngularLock(m_angularlock);
1011 } */
1012 } 1432 }
1013 } 1433 }
1014 1434
@@ -1151,11 +1571,6 @@ namespace OpenSim.Region.Physics.OdePlugin
1151 prm.m_collisionscore = 0; 1571 prm.m_collisionscore = 0;
1152 prm.m_disabled = false; 1572 prm.m_disabled = false;
1153 1573
1154 // The body doesn't already have a finite rotation mode set here
1155 /* ### if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
1156 {
1157 prm.createAMotor(m_angularlock);
1158 } */
1159 prm.Body = Body; 1574 prm.Body = Body;
1160 _parent_scene.addActivePrim(prm); 1575 _parent_scene.addActivePrim(prm);
1161 } 1576 }
@@ -1194,13 +1609,8 @@ namespace OpenSim.Region.Physics.OdePlugin
1194 m_collisionscore = 0; 1609 m_collisionscore = 0;
1195 m_disabled = false; 1610 m_disabled = false;
1196 1611
1197 // The body doesn't already have a finite rotation mode set here
1198 /* ### if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
1199 {
1200 createAMotor(m_angularlock);
1201 } */
1202 d.BodySetPosition(Body, Position.X, Position.Y, Position.Z); 1612 d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);
1203 if (m_vehicle.Type != Vehicle.TYPE_NONE) m_vehicle.Enable(Body, _parent_scene); 1613 if (m_type != Vehicle.TYPE_NONE) Enable(Body, _parent_scene);
1204 _parent_scene.addActivePrim(this); 1614 _parent_scene.addActivePrim(this);
1205 } 1615 }
1206 } 1616 }
@@ -1267,17 +1677,12 @@ namespace OpenSim.Region.Physics.OdePlugin
1267 //Console.WriteLine("childrenPrim.Remove " + odePrim); 1677 //Console.WriteLine("childrenPrim.Remove " + odePrim);
1268 childrenPrim.Remove(odePrim); 1678 childrenPrim.Remove(odePrim);
1269 } 1679 }
1270
1271
1272
1273 1680
1274 if (Body != IntPtr.Zero) 1681 if (Body != IntPtr.Zero)
1275 { 1682 {
1276 _parent_scene.remActivePrim(this); 1683 _parent_scene.remActivePrim(this);
1277 } 1684 }
1278 1685
1279
1280
1281 lock (childrenPrim) 1686 lock (childrenPrim)
1282 { 1687 {
1283 foreach (OdePrim prm in childrenPrim) 1688 foreach (OdePrim prm in childrenPrim)
@@ -1286,8 +1691,6 @@ namespace OpenSim.Region.Physics.OdePlugin
1286 ParentPrim(prm); 1691 ParentPrim(prm);
1287 } 1692 }
1288 } 1693 }
1289
1290
1291 } 1694 }
1292 1695
1293 private void changeSelectedStatus(float timestep) 1696 private void changeSelectedStatus(float timestep)
@@ -1515,17 +1918,22 @@ namespace OpenSim.Region.Physics.OdePlugin
1515 1918
1516 public void changemove(float timestep) 1919 public void changemove(float timestep)
1517 { 1920 {
1921//Console.WriteLine("changemove for {0}", m_primName );
1922
1518 if (m_isphysical) 1923 if (m_isphysical)
1519 { 1924 {
1520 1925//Console.WriteLine("phys {0} {1} {2}", m_disabled, m_taintremove, childPrim);
1521 if (!m_disabled && !m_taintremove && !childPrim) 1926// if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits!
1927 if (!m_taintremove && !childPrim)
1522 { 1928 {
1929//Console.WriteLine("physOK");
1523 if (Body == IntPtr.Zero) 1930 if (Body == IntPtr.Zero)
1524 enableBody(); 1931 enableBody();
1525 //Prim auto disable after 20 frames, 1932 //Prim auto disable after 20 frames,
1526 //if you move it, re-enable the prim manually. 1933 //if you move it, re-enable the prim manually.
1527 if (_parent != null) 1934 if (_parent != null)
1528 { 1935 {
1936//Console.WriteLine("physChild");
1529 if (m_linkJoint != IntPtr.Zero) 1937 if (m_linkJoint != IntPtr.Zero)
1530 { 1938 {
1531 d.JointDestroy(m_linkJoint); 1939 d.JointDestroy(m_linkJoint);
@@ -1534,6 +1942,7 @@ namespace OpenSim.Region.Physics.OdePlugin
1534 } 1942 }
1535 if (Body != IntPtr.Zero) 1943 if (Body != IntPtr.Zero)
1536 { 1944 {
1945//Console.WriteLine("physNotIPZ");
1537 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); 1946 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
1538 1947
1539 if (_parent != null) 1948 if (_parent != null)
@@ -1549,9 +1958,9 @@ Console.WriteLine(" JointCreateFixed");
1549 } 1958 }
1550 } 1959 }
1551 d.BodyEnable(Body); 1960 d.BodyEnable(Body);
1552 if (m_vehicle.Type != Vehicle.TYPE_NONE) 1961 if (m_type != Vehicle.TYPE_NONE)
1553 { 1962 {
1554 m_vehicle.Enable(Body, _parent_scene); 1963 Enable(Body, _parent_scene);
1555 } 1964 }
1556 } 1965 }
1557 else 1966 else
@@ -1566,6 +1975,7 @@ Console.WriteLine(" JointCreateFixed");
1566 } 1975 }
1567 else 1976 else
1568 { 1977 {
1978//Console.WriteLine("NONphys");
1569 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); 1979 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
1570 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); 1980 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
1571 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1981 _parent_scene.waitForSpaceUnlock(m_targetSpace);
@@ -1589,308 +1999,6 @@ Console.WriteLine(" JointCreateFixed");
1589 m_taintposition = _position; 1999 m_taintposition = _position;
1590 } 2000 }
1591 2001
1592 public void Move(float timestep)
1593 {
1594 float fx = 0;
1595 float fy = 0;
1596 float fz = 0;
1597
1598 frcount++; // used to limit debug comment output
1599 if (frcount > 100)
1600 frcount = 0;
1601
1602 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims.
1603 {
1604//if(frcount == 0) Console.WriteLine("Move " + m_primName + " VTyp " + m_vehicle.Type +
1605 // " usePID=" + m_usePID + " seHover=" + m_useHoverPID + " useAPID=" + m_useAPID);
1606 if (m_vehicle.Type != Vehicle.TYPE_NONE)
1607 {
1608 // 'VEHICLES' are dealt with in ODEDynamics.cs
1609 m_vehicle.Step(timestep, _parent_scene);
1610 }
1611 else
1612 {
1613 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
1614 // NON-'VEHICLES' are dealt with here
1615 // m_angularlock = <1,1,1> means no lock. a 0 on axis means locked.
1616
1617// NB this may be wrong - may lock global axis! Should be LOCAL axis!
1618 if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.One, 0.003f))
1619 {
1620 d.Vector3 avel2 = d.BodyGetAngularVel(Body);
1621 if (m_angularlock.X == 0)
1622 avel2.X = 0;
1623 if (m_angularlock.Y == 0)
1624 avel2.Y = 0;
1625 if (m_angularlock.Z == 0)
1626 avel2.Z = 0;
1627 d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
1628 }
1629 //float PID_P = 900.0f;
1630
1631 float m_mass = CalculateMass();
1632
1633// fz = 0f;
1634 //m_log.Info(m_collisionFlags.ToString());
1635
1636
1637 //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle.
1638 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
1639 // NB Prims in ODE are no subject to global gravity
1640 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass
1641
1642 if (m_usePID)
1643 {
1644//if(frcount == 0) Console.WriteLine("PID " + m_primName);
1645 // KF - this is for object MoveToTarget.
1646
1647 //if (!d.BodyIsEnabled(Body))
1648 //d.BodySetForce(Body, 0f, 0f, 0f);
1649
1650 // no lock; for now it's only called from within Simulate()
1651
1652 // If the PID Controller isn't active then we set our force
1653 // calculating base velocity to the current position
1654
1655 if ((m_PIDTau < 1) && (m_PIDTau != 0))
1656 {
1657 //PID_G = PID_G / m_PIDTau;
1658 m_PIDTau = 1;
1659 }
1660
1661 if ((PID_G - m_PIDTau) <= 0)
1662 {
1663 PID_G = m_PIDTau + 1;
1664 }
1665 //PidStatus = true;
1666
1667 // PhysicsVector vec = new PhysicsVector();
1668 d.Vector3 vel = d.BodyGetLinearVel(Body);
1669
1670 d.Vector3 pos = d.BodyGetPosition(Body);
1671 _target_velocity =
1672 new Vector3(
1673 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
1674 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
1675 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
1676 );
1677
1678 // if velocity is zero, use position control; otherwise, velocity control
1679
1680 if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
1681 {
1682 // keep track of where we stopped. No more slippin' & slidin'
1683
1684 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1685 // react to the physics scene by moving it's position.
1686 // Avatar to Avatar collisions
1687 // Prim to avatar collisions
1688
1689 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
1690 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
1691 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
1692 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
1693 d.BodySetLinearVel(Body, 0, 0, 0);
1694 d.BodyAddForce(Body, 0, 0, fz);
1695 return;
1696 }
1697 else
1698 {
1699 _zeroFlag = false;
1700
1701 // We're flying and colliding with something
1702 fx = ((_target_velocity.X) - vel.X) * (PID_D);
1703 fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
1704
1705 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
1706
1707 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
1708 }
1709 } // end if (m_usePID)
1710
1711 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
1712 if (m_useHoverPID && !m_usePID)
1713 {
1714//Console.WriteLine("Hover " + m_primName);
1715
1716 // If we're using the PID controller, then we have no gravity
1717 fz = (-1 * _parent_scene.gravityz) * m_mass;
1718
1719 // no lock; for now it's only called from within Simulate()
1720
1721 // If the PID Controller isn't active then we set our force
1722 // calculating base velocity to the current position
1723
1724 if ((m_PIDTau < 1))
1725 {
1726 PID_G = PID_G / m_PIDTau;
1727 }
1728
1729 if ((PID_G - m_PIDTau) <= 0)
1730 {
1731 PID_G = m_PIDTau + 1;
1732 }
1733
1734
1735 // Where are we, and where are we headed?
1736 d.Vector3 pos = d.BodyGetPosition(Body);
1737 d.Vector3 vel = d.BodyGetLinearVel(Body);
1738
1739
1740 // Non-Vehicles have a limited set of Hover options.
1741 // determine what our target height really is based on HoverType
1742 switch (m_PIDHoverType)
1743 {
1744 case PIDHoverType.Ground:
1745 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
1746 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
1747 break;
1748 case PIDHoverType.GroundAndWater:
1749 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
1750 m_waterHeight = _parent_scene.GetWaterLevel();
1751 if (m_groundHeight > m_waterHeight)
1752 {
1753 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
1754 }
1755 else
1756 {
1757 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
1758 }
1759 break;
1760
1761 } // end switch (m_PIDHoverType)
1762
1763
1764 _target_velocity =
1765 new Vector3(0.0f, 0.0f,
1766 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
1767 );
1768
1769 // if velocity is zero, use position control; otherwise, velocity control
1770
1771 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
1772 {
1773 // keep track of where we stopped. No more slippin' & slidin'
1774
1775 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1776 // react to the physics scene by moving it's position.
1777 // Avatar to Avatar collisions
1778 // Prim to avatar collisions
1779
1780 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
1781 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
1782 d.BodyAddForce(Body, 0, 0, fz);
1783 //KF this prevents furthur motions return;
1784 }
1785 else
1786 {
1787 _zeroFlag = false;
1788
1789 // We're flying and colliding with something
1790 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
1791 }
1792 } // end m_useHoverPID && !m_usePID
1793
1794 if (m_useAPID)
1795 {
1796 // RotLookAt, apparently overrides all other rotation sources. Inputs:
1797 // Quaternion m_APIDTarget
1798 // float m_APIDStrength // From SL experiments, this is the time to get there
1799 // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly
1800 // Also in SL the mass of the object has no effect on time to get there.
1801 // Factors:
1802//if(frcount == 0) Console.WriteLine("APID ");
1803 // get present body rotation
1804 float limit = 1.0f;
1805 float scaler = 50f; // adjusts damping time
1806 float RLAservo = 0f;
1807
1808 d.Quaternion rot = d.BodyGetQuaternion(Body);
1809 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
1810 Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget;
1811 float diff_angle;
1812 Vector3 diff_axis;
1813 rot_diff.GetAxisAngle(out diff_axis, out diff_angle);
1814 diff_axis.Normalize();
1815 if(diff_angle > 0.01f) // diff_angle is always +ve
1816 {
1817// PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z);
1818 Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z);
1819 rotforce = rotforce * rotq;
1820 if(diff_angle > limit) diff_angle = limit; // cap the rotate rate
1821// RLAservo = timestep / m_APIDStrength * m_mass * scaler;
1822 // rotforce = rotforce * RLAservo * diff_angle ;
1823 // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z);
1824 RLAservo = timestep / m_APIDStrength * scaler;
1825 rotforce = rotforce * RLAservo * diff_angle ;
1826
1827 if (m_angularlock.X == 0)
1828 rotforce.X = 0;
1829 if (m_angularlock.Y == 0)
1830 rotforce.Y = 0;
1831 if (m_angularlock.Z == 0)
1832 rotforce.Z = 0;
1833
1834
1835
1836
1837 d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z);
1838//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo);
1839 }
1840//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle);
1841 } // end m_useAPID
1842
1843 fx *= m_mass;
1844 fy *= m_mass;
1845 //fz *= m_mass;
1846
1847 fx += m_force.X;
1848 fy += m_force.Y;
1849 fz += m_force.Z;
1850
1851 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
1852 if (fx != 0 || fy != 0 || fz != 0)
1853 {
1854 //m_taintdisable = true;
1855 //base.RaiseOutOfBounds(Position);
1856 //d.BodySetLinearVel(Body, fx, fy, 0f);
1857 if (!d.BodyIsEnabled(Body))
1858 {
1859 // A physical body at rest on a surface will auto-disable after a while,
1860 // this appears to re-enable it incase the surface it is upon vanishes,
1861 // and the body should fall again.
1862 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1863 d.BodySetForce(Body, 0, 0, 0);
1864 enableBodySoft();
1865 }
1866
1867 // 35x10 = 350n times the mass per second applied maximum.
1868 float nmax = 35f * m_mass;
1869 float nmin = -35f * m_mass;
1870
1871
1872 if (fx > nmax)
1873 fx = nmax;
1874 if (fx < nmin)
1875 fx = nmin;
1876 if (fy > nmax)
1877 fy = nmax;
1878 if (fy < nmin)
1879 fy = nmin;
1880 d.BodyAddForce(Body, fx, fy, fz);
1881//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
1882 }
1883 }
1884 }
1885 else
1886 { // is not physical, or is not a body or is selected
1887 // _zeroPosition = d.BodyGetPosition(Body);
1888 return;
1889//Console.WriteLine("Nothing " + m_primName);
1890
1891 }
1892 }
1893
1894 2002
1895 2003
1896 public void rotate(float timestep) 2004 public void rotate(float timestep)
@@ -1904,11 +2012,6 @@ Console.WriteLine(" JointCreateFixed");
1904 { 2012 {
1905 // KF: If this is a root prim do BodySet 2013 // KF: If this is a root prim do BodySet
1906 d.BodySetQuaternion(Body, ref myrot); 2014 d.BodySetQuaternion(Body, ref myrot);
1907 /* ### if (m_isphysical)
1908 {
1909 if (!m_angularlock.ApproxEquals(Vector3.One, 0f))
1910 createAMotor(m_angularlock);
1911 } */
1912 } 2015 }
1913 else 2016 else
1914 { 2017 {
@@ -2319,385 +2422,9 @@ Console.WriteLine(" JointCreateFixed");
2319 m_taintVelocity = Vector3.Zero; 2422 m_taintVelocity = Vector3.Zero;
2320 } 2423 }
2321 2424
2322 public override bool IsPhysical
2323 {
2324 get { return m_isphysical; }
2325 set
2326 {
2327 m_isphysical = value;
2328 if (!m_isphysical)
2329 { // Zero the remembered last velocity
2330 m_lastVelocity = Vector3.Zero;
2331 if (m_vehicle.Type != Vehicle.TYPE_NONE) m_vehicle.Halt();
2332 }
2333 }
2334 }
2335
2336 public void setPrimForRemoval()
2337 {
2338 m_taintremove = true;
2339 }
2340
2341 public override bool Flying
2342 {
2343 // no flying prims for you
2344 get { return false; }
2345 set { }
2346 }
2347
2348 public override bool IsColliding
2349 {
2350 get { return iscolliding; }
2351 set { iscolliding = value; }
2352 }
2353
2354 public override bool CollidingGround
2355 {
2356 get { return false; }
2357 set { return; }
2358 }
2359
2360 public override bool CollidingObj
2361 {
2362 get { return false; }
2363 set { return; }
2364 }
2365
2366 public override bool ThrottleUpdates
2367 {
2368 get { return m_throttleUpdates; }
2369 set { m_throttleUpdates = value; }
2370 }
2371
2372 public override bool Stopped
2373 {
2374 get { return _zeroFlag; }
2375 }
2376
2377 public override Vector3 Position
2378 {
2379 get { return _position; }
2380
2381 set { _position = value;
2382 //m_log.Info("[PHYSICS]: " + _position.ToString());
2383 }
2384 }
2385
2386 public override Vector3 Size
2387 {
2388 get { return _size; }
2389 set
2390 {
2391 if (value.IsFinite())
2392 {
2393 _size = value;
2394 }
2395 else
2396 {
2397 m_log.Warn("[PHYSICS]: Got NaN Size on object");
2398 }
2399 }
2400 }
2401
2402 public override float Mass
2403 {
2404 get { return CalculateMass(); }
2405 }
2406
2407 public override Vector3 Force
2408 {
2409 //get { return Vector3.Zero; }
2410 get { return m_force; }
2411 set
2412 {
2413 if (value.IsFinite())
2414 {
2415 m_force = value;
2416 }
2417 else
2418 {
2419 m_log.Warn("[PHYSICS]: NaN in Force Applied to an Object");
2420 }
2421 }
2422 }
2423
2424 public override int VehicleType
2425 {
2426 get { return (int)m_vehicle.Type; }
2427 set { m_vehicle.ProcessTypeChange((Vehicle)value); }
2428 }
2429
2430 public override void VehicleFloatParam(int param, float value)
2431 {
2432 m_vehicle.ProcessFloatVehicleParam((Vehicle) param, value);
2433 }
2434
2435 public override void VehicleVectorParam(int param, Vector3 value)
2436 {
2437 m_vehicle.ProcessVectorVehicleParam((Vehicle) param, value);
2438 }
2439
2440 public override void VehicleRotationParam(int param, Quaternion rotation)
2441 {
2442 m_vehicle.ProcessRotationVehicleParam((Vehicle) param, rotation);
2443 }
2444
2445 public override void VehicleFlagsSet(int flags)
2446 {
2447 m_vehicle.ProcessFlagsVehicleSet(flags);
2448 }
2449
2450 public override void VehicleFlagsRemove(int flags)
2451 {
2452 m_vehicle.ProcessFlagsVehicleRemove(flags);
2453 }
2454
2455 public override void SetVolumeDetect(int param)
2456 {
2457 lock (_parent_scene.OdeLock)
2458 {
2459 m_isVolumeDetect = (param!=0);
2460 }
2461 }
2462
2463 public override Vector3 CenterOfMass
2464 {
2465 get { return Vector3.Zero; }
2466 }
2467
2468 public override Vector3 GeometricCenter
2469 {
2470 get { return Vector3.Zero; }
2471 }
2472
2473 public override PrimitiveBaseShape Shape
2474 {
2475 set
2476 {
2477 _pbs = value;
2478 m_taintshape = true;
2479 }
2480 }
2481
2482 public override Vector3 Velocity
2483 {
2484 get
2485 {
2486 // Averate previous velocity with the new one so
2487 // client object interpolation works a 'little' better
2488 if (_zeroFlag)
2489 return Vector3.Zero;
2490
2491 Vector3 returnVelocity = Vector3.Zero;
2492 returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2;
2493 returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2;
2494 returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2;
2495 return returnVelocity;
2496 }
2497 set
2498 {
2499 if (value.IsFinite())
2500 {
2501 _velocity = value;
2502
2503 m_taintVelocity = value;
2504 _parent_scene.AddPhysicsActorTaint(this);
2505 }
2506 else
2507 {
2508 m_log.Warn("[PHYSICS]: Got NaN Velocity in Object");
2509 }
2510
2511 }
2512 }
2513
2514 public override Vector3 Torque
2515 {
2516 get
2517 {
2518 if (!m_isphysical || Body == IntPtr.Zero)
2519 return Vector3.Zero;
2520
2521 return _torque;
2522 }
2523
2524 set
2525 {
2526 if (value.IsFinite())
2527 {
2528 m_taintTorque = value;
2529 _parent_scene.AddPhysicsActorTaint(this);
2530 }
2531 else
2532 {
2533 m_log.Warn("[PHYSICS]: Got NaN Torque in Object");
2534 }
2535 }
2536 }
2537/*
2538 public Vector3 AngularLock
2539 {
2540 get { return m_angularlock; }
2541 set { }
2542 }
2543
2544*/
2545 public override float CollisionScore
2546 {
2547 get { return m_collisionscore; }
2548 set { m_collisionscore = value; }
2549 }
2550
2551 public override bool Kinematic
2552 {
2553 get { return false; }
2554 set { }
2555 }
2556
2557 public override Quaternion Orientation
2558 {
2559 get { return _orientation; }
2560 set
2561 {
2562 if (QuaternionIsFinite(value))
2563 {
2564 _orientation = value;
2565 }
2566 else
2567 m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object");
2568
2569 }
2570 }
2571
2572 internal static bool QuaternionIsFinite(Quaternion q)
2573 {
2574 if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
2575 return false;
2576 if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
2577 return false;
2578 if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
2579 return false;
2580 if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
2581 return false;
2582 return true;
2583 }
2584
2585 public override Vector3 Acceleration
2586 {
2587 get { return _acceleration; }
2588 }
2589
2590
2591 public void SetAcceleration(Vector3 accel)
2592 {
2593 _acceleration = accel;
2594 }
2595
2596 public override void AddForce(Vector3 force, bool pushforce)
2597 {
2598 if (force.IsFinite())
2599 {
2600 lock (m_forcelist)
2601 m_forcelist.Add(force);
2602
2603 m_taintforce = true;
2604 }
2605 else
2606 {
2607 m_log.Warn("[PHYSICS]: Got Invalid linear force vector from Scene in Object");
2608 }
2609 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
2610 }
2611
2612 public override void AddAngularForce(Vector3 force, bool pushforce)
2613 {
2614 if (force.IsFinite())
2615 {
2616 m_angularforcelist.Add(force);
2617 m_taintaddangularforce = true;
2618 }
2619 else
2620 {
2621 m_log.Warn("[PHYSICS]: Got Invalid Angular force vector from Scene in Object");
2622 }
2623 }
2624
2625 public override Vector3 RotationalVelocity
2626 {
2627 get
2628 {
2629/* Vector3 pv = Vector3.Zero;
2630 if (_zeroFlag)
2631 return pv;
2632 m_lastUpdateSent = false;
2633
2634 if (m_rotationalVelocity.ApproxEquals(pv, 0.2f))
2635 return pv;
2636*/
2637 return m_rotationalVelocity;
2638 }
2639 set
2640 {
2641 if (value.IsFinite())
2642 {
2643 m_rotationalVelocity = value;
2644 }
2645 else
2646 {
2647 m_log.Warn("[PHYSICS]: Got NaN RotationalVelocity in Object");
2648 }
2649 }
2650 }
2651
2652 public override void CrossingFailure()
2653 {
2654 m_crossingfailures++;
2655 if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
2656 {
2657 base.RaiseOutOfBounds(_position);
2658 return;
2659 }
2660 else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
2661 {
2662 m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName);
2663 }
2664 }
2665
2666 public override float Buoyancy
2667 {
2668 get { return m_buoyancy; }
2669 set { m_buoyancy = value; }
2670 }
2671
2672 public override void link(PhysicsActor obj)
2673 {
2674 m_taintparent = obj;
2675 }
2676
2677 public override void delink()
2678 {
2679 m_taintparent = null;
2680 }
2681
2682 public override void LockAngularMotion(Vector3 axis)
2683 {
2684 // reverse the zero/non zero values for ODE.
2685 if (axis.IsFinite())
2686 {
2687 axis.X = (axis.X > 0) ? 1f : 0f;
2688 axis.Y = (axis.Y > 0) ? 1f : 0f;
2689 axis.Z = (axis.Z > 0) ? 1f : 0f;
2690 m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
2691 m_taintAngularLock = axis;
2692 }
2693 else
2694 {
2695 m_log.Warn("[PHYSICS]: Got NaN locking axis from Scene on Object");
2696 }
2697 }
2698
2699 public void UpdatePositionAndVelocity() 2425 public void UpdatePositionAndVelocity()
2700 { 2426 {
2427 return; // moved to the MOVE() method
2701 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! 2428 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
2702 if (_parent == null) 2429 if (_parent == null)
2703 { 2430 {
@@ -2909,167 +2636,6 @@ Console.WriteLine(" JointCreateFixed");
2909 } 2636 }
2910 } 2637 }
2911 2638
2912 public override bool FloatOnWater
2913 {
2914 set {
2915 m_taintCollidesWater = value;
2916 _parent_scene.AddPhysicsActorTaint(this);
2917 }
2918 }
2919
2920 public override void SetMomentum(Vector3 momentum)
2921 {
2922 }
2923
2924 public override Vector3 PIDTarget
2925 {
2926 set
2927 {
2928 if (value.IsFinite())
2929 {
2930 m_PIDTarget = value;
2931 }
2932 else
2933 m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object");
2934 }
2935 }
2936 public override bool PIDActive { set { m_usePID = value; } }
2937 public override float PIDTau { set { m_PIDTau = value; } }
2938
2939 // For RotLookAt
2940 public override Quaternion APIDTarget { set { m_APIDTarget = value; } }
2941 public override bool APIDActive { set { m_useAPID = value; } }
2942 public override float APIDStrength { set { m_APIDStrength = value; } }
2943 public override float APIDDamping { set { m_APIDDamping = value; } }
2944
2945 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
2946 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
2947 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
2948 public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
2949
2950 private void createAMotor(Vector3 axis) // ##*
2951 {
2952Console.WriteLine(" createAMotor called! ----------------------------");
2953 if (Body == IntPtr.Zero)
2954 return;
2955
2956 if (Amotor != IntPtr.Zero)
2957 {
2958 d.JointDestroy(Amotor);
2959 Amotor = IntPtr.Zero;
2960 }
2961
2962 float axisnum = 3;
2963
2964 axisnum = (axisnum - (axis.X + axis.Y + axis.Z));
2965
2966 // PhysicsVector totalSize = new PhysicsVector(_size.X, _size.Y, _size.Z);
2967
2968
2969 // Inverse Inertia Matrix, set the X, Y, and/r Z inertia to 0 then invert it again.
2970 d.Mass objMass;
2971 d.MassSetZero(out objMass);
2972 DMassCopy(ref pMass, ref objMass);
2973
2974 //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);
2975
2976 Matrix4 dMassMat = FromDMass(objMass);
2977
2978 Matrix4 mathmat = Inverse(dMassMat);
2979
2980 /*
2981 //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]);
2982
2983 mathmat = Inverse(mathmat);
2984
2985
2986 objMass = FromMatrix4(mathmat, ref objMass);
2987 //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);
2988
2989 mathmat = Inverse(mathmat);
2990 */
2991 if (axis.X == 0)
2992 {
2993 mathmat.M33 = 50.0000001f;
2994 //objMass.I.M22 = 0;
2995 }
2996 if (axis.Y == 0)
2997 {
2998 mathmat.M22 = 50.0000001f;
2999 //objMass.I.M11 = 0;
3000 }
3001 if (axis.Z == 0)
3002 {
3003 mathmat.M11 = 50.0000001f;
3004 //objMass.I.M00 = 0;
3005 }
3006
3007
3008
3009 mathmat = Inverse(mathmat);
3010 objMass = FromMatrix4(mathmat, ref objMass);
3011 //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);
3012
3013 //return;
3014 if (d.MassCheck(ref objMass))
3015 {
3016 d.BodySetMass(Body, ref objMass);
3017 }
3018 else
3019 {
3020 //m_log.Debug("[PHYSICS]: Mass invalid, ignoring");
3021 }
3022
3023 if (axisnum <= 0)
3024 return;
3025 // int dAMotorEuler = 1;
3026
3027 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
3028 d.JointAttach(Amotor, Body, IntPtr.Zero);
3029 d.JointSetAMotorMode(Amotor, 0);
3030
3031 d.JointSetAMotorNumAxes(Amotor,(int)axisnum);
3032 int i = 0;
3033
3034 if (axis.X == 0)
3035 {
3036 d.JointSetAMotorAxis(Amotor, i, 0, 1, 0, 0);
3037 i++;
3038 }
3039
3040 if (axis.Y == 0)
3041 {
3042 d.JointSetAMotorAxis(Amotor, i, 0, 0, 1, 0);
3043 i++;
3044 }
3045
3046 if (axis.Z == 0)
3047 {
3048 d.JointSetAMotorAxis(Amotor, i, 0, 0, 0, 1);
3049 i++;
3050 }
3051
3052 for (int j = 0; j < (int)axisnum; j++)
3053 {
3054 //d.JointSetAMotorAngle(Amotor, j, 0);
3055 }
3056
3057 //d.JointSetAMotorAngle(Amotor, 1, 0);
3058 //d.JointSetAMotorAngle(Amotor, 2, 0);
3059
3060 // These lowstops and high stops are effectively (no wiggle room)
3061 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0f);
3062 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f);
3063 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0f);
3064 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f);
3065 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f);
3066 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f);
3067 //d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 9000f);
3068 d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
3069 d.JointSetAMotorParam(Amotor, (int)dParam.FMax, Mass * 50f);//
3070
3071 }
3072
3073 public Matrix4 FromDMass(d.Mass pMass) 2639 public Matrix4 FromDMass(d.Mass pMass)
3074 { 2640 {
3075 Matrix4 obj; 2641 Matrix4 obj;
@@ -3318,5 +2884,1135 @@ Console.WriteLine(" createAMotor called! ----------------------------");
3318 m_material = pMaterial; 2884 m_material = pMaterial;
3319 } 2885 }
3320 2886
3321 } 2887
2888
2889
2890
2891
2892 internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
2893 {
2894 switch (pParam)
2895 {
2896 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
2897 if (pValue < 0.01f) pValue = 0.01f;
2898 // m_angularDeflectionEfficiency = pValue;
2899 break;
2900 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
2901 if (pValue < 0.01f) pValue = 0.01f;
2902 // m_angularDeflectionTimescale = pValue;
2903 break;
2904 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
2905 if (pValue < 0.01f) pValue = 0.01f;
2906 m_angularMotorDecayTimescale = pValue;
2907 break;
2908 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
2909 if (pValue < 0.01f) pValue = 0.01f;
2910 m_angularMotorTimescale = pValue;
2911 break;
2912 case Vehicle.BANKING_EFFICIENCY:
2913 if (pValue < 0.01f) pValue = 0.01f;
2914 // m_bankingEfficiency = pValue;
2915 break;
2916 case Vehicle.BANKING_MIX:
2917 if (pValue < 0.01f) pValue = 0.01f;
2918 // m_bankingMix = pValue;
2919 break;
2920 case Vehicle.BANKING_TIMESCALE:
2921 if (pValue < 0.01f) pValue = 0.01f;
2922 // m_bankingTimescale = pValue;
2923 break;
2924 case Vehicle.BUOYANCY:
2925 if (pValue < -1f) pValue = -1f;
2926 if (pValue > 1f) pValue = 1f;
2927 m_VehicleBuoyancy = pValue;
2928 break;
2929// case Vehicle.HOVER_EFFICIENCY:
2930// if (pValue < 0f) pValue = 0f;
2931// if (pValue > 1f) pValue = 1f;
2932// m_VhoverEfficiency = pValue;
2933// break;
2934 case Vehicle.HOVER_HEIGHT:
2935 m_VhoverHeight = pValue;
2936 break;
2937 case Vehicle.HOVER_TIMESCALE:
2938 if (pValue < 0.01f) pValue = 0.01f;
2939 m_VhoverTimescale = pValue;
2940 break;
2941 case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
2942 if (pValue < 0.01f) pValue = 0.01f;
2943 // m_linearDeflectionEfficiency = pValue;
2944 break;
2945 case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
2946 if (pValue < 0.01f) pValue = 0.01f;
2947 // m_linearDeflectionTimescale = pValue;
2948 break;
2949 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
2950 if (pValue < 0.01f) pValue = 0.01f;
2951 m_linearMotorDecayTimescale = pValue;
2952 break;
2953 case Vehicle.LINEAR_MOTOR_TIMESCALE:
2954 if (pValue < 0.01f) pValue = 0.01f;
2955 m_linearMotorTimescale = pValue;
2956 break;
2957 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
2958 if (pValue < 0.1f) pValue = 0.1f; // Less goes unstable
2959 if (pValue > 1.0f) pValue = 1.0f;
2960 m_verticalAttractionEfficiency = pValue;
2961 break;
2962 case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
2963 if (pValue < 0.01f) pValue = 0.01f;
2964 m_verticalAttractionTimescale = pValue;
2965 break;
2966
2967 // These are vector properties but the engine lets you use a single float value to
2968 // set all of the components to the same value
2969 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
2970 if (pValue > 30f) pValue = 30f;
2971 if (pValue < 0.1f) pValue = 0.1f;
2972 m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
2973 break;
2974 case Vehicle.ANGULAR_MOTOR_DIRECTION:
2975 m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
2976 UpdateAngDecay();
2977 break;
2978 case Vehicle.LINEAR_FRICTION_TIMESCALE:
2979 m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
2980 break;
2981 case Vehicle.LINEAR_MOTOR_DIRECTION:
2982 m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
2983 UpdateLinDecay();
2984 break;
2985 case Vehicle.LINEAR_MOTOR_OFFSET:
2986 // m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
2987 break;
2988
2989 }
2990
2991 }//end ProcessFloatVehicleParam
2992
2993 internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
2994 {
2995 switch (pParam)
2996 {
2997 case Vehicle.ANGULAR_FRICTION_TIMESCALE:
2998 if (pValue.X > 30f) pValue.X = 30f;
2999 if (pValue.X < 0.1f) pValue.X = 0.1f;
3000 if (pValue.Y > 30f) pValue.Y = 30f;
3001 if (pValue.Y < 0.1f) pValue.Y = 0.1f;
3002 if (pValue.Z > 30f) pValue.Z = 30f;
3003 if (pValue.Z < 0.1f) pValue.Z = 0.1f;
3004 m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
3005 break;
3006 case Vehicle.ANGULAR_MOTOR_DIRECTION:
3007 m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
3008 // Limit requested angular speed to 2 rps= 4 pi rads/sec
3009 if(m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f;
3010 if(m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f;
3011 if(m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f;
3012 if(m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f;
3013 if(m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f;
3014 if(m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f;
3015 UpdateAngDecay();
3016 break;
3017 case Vehicle.LINEAR_FRICTION_TIMESCALE:
3018 m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
3019 break;
3020 case Vehicle.LINEAR_MOTOR_DIRECTION:
3021 m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); // velocity requested by LSL, for max limiting
3022 UpdateLinDecay();
3023 break;
3024 case Vehicle.LINEAR_MOTOR_OFFSET:
3025 // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
3026 break;
3027 }
3028
3029 }//end ProcessVectorVehicleParam
3030
3031 internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
3032 {
3033 switch (pParam)
3034 {
3035 case Vehicle.REFERENCE_FRAME:
3036 // m_referenceFrame = pValue;
3037 break;
3038 }
3039
3040 }//end ProcessRotationVehicleParam
3041
3042 internal void ProcessFlagsVehicleSet(int flags)
3043 {
3044 m_flags |= (VehicleFlag)flags;
3045 }
3046
3047 internal void ProcessFlagsVehicleRemove(int flags)
3048 {
3049 m_flags &= ~((VehicleFlag)flags);
3050 }
3051
3052 internal void ProcessTypeChange(Vehicle pType)
3053 {
3054 // Set Defaults For Type
3055 m_type = pType;
3056 switch (pType)
3057 {
3058 case Vehicle.TYPE_SLED:
3059 m_linearFrictionTimescale = new Vector3(30, 1, 1000);
3060 m_angularFrictionTimescale = new Vector3(30, 30, 30);
3061// m_lLinMotorVel = Vector3.Zero;
3062 m_linearMotorTimescale = 1000;
3063 m_linearMotorDecayTimescale = 120;
3064 m_angularMotorDirection = Vector3.Zero;
3065 m_angularMotorDVel = Vector3.Zero;
3066 m_angularMotorTimescale = 1000;
3067 m_angularMotorDecayTimescale = 120;
3068 m_VhoverHeight = 0;
3069// m_VhoverEfficiency = 1;
3070 m_VhoverTimescale = 10;
3071 m_VehicleBuoyancy = 0;
3072 // m_linearDeflectionEfficiency = 1;
3073 // m_linearDeflectionTimescale = 1;
3074 // m_angularDeflectionEfficiency = 1;
3075 // m_angularDeflectionTimescale = 1000;
3076 // m_bankingEfficiency = 0;
3077 // m_bankingMix = 1;
3078 // m_bankingTimescale = 10;
3079 // m_referenceFrame = Quaternion.Identity;
3080 m_flags &=
3081 ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
3082 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
3083 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
3084 break;
3085 case Vehicle.TYPE_CAR:
3086 m_linearFrictionTimescale = new Vector3(100, 2, 1000);
3087 m_angularFrictionTimescale = new Vector3(30, 30, 30); // was 1000, but sl max frict time is 30.
3088// m_lLinMotorVel = Vector3.Zero;
3089 m_linearMotorTimescale = 1;
3090 m_linearMotorDecayTimescale = 60;
3091 m_angularMotorDirection = Vector3.Zero;
3092 m_angularMotorDVel = Vector3.Zero;
3093 m_angularMotorTimescale = 1;
3094 m_angularMotorDecayTimescale = 0.8f;
3095 m_VhoverHeight = 0;
3096// m_VhoverEfficiency = 0;
3097 m_VhoverTimescale = 1000;
3098 m_VehicleBuoyancy = 0;
3099 // // m_linearDeflectionEfficiency = 1;
3100 // // m_linearDeflectionTimescale = 2;
3101 // // m_angularDeflectionEfficiency = 0;
3102 // m_angularDeflectionTimescale = 10;
3103 m_verticalAttractionEfficiency = 1f;
3104 m_verticalAttractionTimescale = 10f;
3105 // m_bankingEfficiency = -0.2f;
3106 // m_bankingMix = 1;
3107 // m_bankingTimescale = 1;
3108 // m_referenceFrame = Quaternion.Identity;
3109 m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
3110 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY |
3111 VehicleFlag.LIMIT_MOTOR_UP);
3112 break;
3113 case Vehicle.TYPE_BOAT:
3114 m_linearFrictionTimescale = new Vector3(10, 3, 2);
3115 m_angularFrictionTimescale = new Vector3(10,10,10);
3116// m_lLinMotorVel = Vector3.Zero;
3117 m_linearMotorTimescale = 5;
3118 m_linearMotorDecayTimescale = 60;
3119 m_angularMotorDirection = Vector3.Zero;
3120 m_angularMotorDVel = Vector3.Zero;
3121 m_angularMotorTimescale = 4;
3122 m_angularMotorDecayTimescale = 4;
3123 m_VhoverHeight = 0;
3124// m_VhoverEfficiency = 0.5f;
3125 m_VhoverTimescale = 2;
3126 m_VehicleBuoyancy = 1;
3127 // m_linearDeflectionEfficiency = 0.5f;
3128 // m_linearDeflectionTimescale = 3;
3129 // m_angularDeflectionEfficiency = 0.5f;
3130 // m_angularDeflectionTimescale = 5;
3131 m_verticalAttractionEfficiency = 0.5f;
3132 m_verticalAttractionTimescale = 5f;
3133 // m_bankingEfficiency = -0.3f;
3134 // m_bankingMix = 0.8f;
3135 // m_bankingTimescale = 1;
3136 // m_referenceFrame = Quaternion.Identity;
3137 m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY |
3138 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
3139 m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY |
3140 VehicleFlag.LIMIT_MOTOR_UP);
3141 break;
3142 case Vehicle.TYPE_AIRPLANE:
3143 m_linearFrictionTimescale = new Vector3(200, 10, 5);
3144 m_angularFrictionTimescale = new Vector3(20, 20, 20);
3145// m_lLinMotorVel = Vector3.Zero;
3146 m_linearMotorTimescale = 2;
3147 m_linearMotorDecayTimescale = 60;
3148 m_angularMotorDirection = Vector3.Zero;
3149 m_angularMotorDVel = Vector3.Zero;
3150 m_angularMotorTimescale = 4;
3151 m_angularMotorDecayTimescale = 4;
3152 m_VhoverHeight = 0;
3153// m_VhoverEfficiency = 0.5f;
3154 m_VhoverTimescale = 1000;
3155 m_VehicleBuoyancy = 0;
3156 // m_linearDeflectionEfficiency = 0.5f;
3157 // m_linearDeflectionTimescale = 3;
3158 // m_angularDeflectionEfficiency = 1;
3159 // m_angularDeflectionTimescale = 2;
3160 m_verticalAttractionEfficiency = 0.9f;
3161 m_verticalAttractionTimescale = 2f;
3162 // m_bankingEfficiency = 1;
3163 // m_bankingMix = 0.7f;
3164 // m_bankingTimescale = 2;
3165 // m_referenceFrame = Quaternion.Identity;
3166 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
3167 VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
3168 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
3169 break;
3170 case Vehicle.TYPE_BALLOON:
3171 m_linearFrictionTimescale = new Vector3(5, 5, 5);
3172 m_angularFrictionTimescale = new Vector3(10, 10, 10);
3173 m_linearMotorTimescale = 5;
3174 m_linearMotorDecayTimescale = 60;
3175 m_angularMotorDirection = Vector3.Zero;
3176 m_angularMotorDVel = Vector3.Zero;
3177 m_angularMotorTimescale = 6;
3178 m_angularMotorDecayTimescale = 10;
3179 m_VhoverHeight = 5;
3180// m_VhoverEfficiency = 0.8f;
3181 m_VhoverTimescale = 10;
3182 m_VehicleBuoyancy = 1;
3183 // m_linearDeflectionEfficiency = 0;
3184 // m_linearDeflectionTimescale = 5;
3185 // m_angularDeflectionEfficiency = 0;
3186 // m_angularDeflectionTimescale = 5;
3187 m_verticalAttractionEfficiency = 1f;
3188 m_verticalAttractionTimescale = 100f;
3189 // m_bankingEfficiency = 0;
3190 // m_bankingMix = 0.7f;
3191 // m_bankingTimescale = 5;
3192 // m_referenceFrame = Quaternion.Identity;
3193 m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
3194 VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
3195 m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
3196 break;
3197
3198 }
3199 }//end SetDefaultsForType
3200
3201 internal void Enable(IntPtr pBody, OdeScene pParentScene)
3202 {
3203 if (m_type == Vehicle.TYPE_NONE)
3204 return;
3205
3206 m_body = pBody;
3207 }
3208
3209
3210 internal void Halt()
3211 { // Kill all motions, when non-physical
3212 m_linearMotorDirection = Vector3.Zero;
3213 m_lLinMotorDVel = Vector3.Zero;
3214 m_lLinObjectVel = Vector3.Zero;
3215 m_wLinObjectVel = Vector3.Zero;
3216 m_angularMotorDirection = Vector3.Zero;
3217 m_lastAngularVelocity = Vector3.Zero;
3218 m_angularMotorDVel = Vector3.Zero;
3219 }
3220
3221 private void UpdateLinDecay()
3222 {
3223 if (Math.Abs(m_linearMotorDirection.X) > Math.Abs(m_lLinMotorDVel.X)) m_lLinMotorDVel.X = m_linearMotorDirection.X;
3224 if (Math.Abs(m_linearMotorDirection.Y) > Math.Abs(m_lLinMotorDVel.Y)) m_lLinMotorDVel.Y = m_linearMotorDirection.Y;
3225 if (Math.Abs(m_linearMotorDirection.Z) > Math.Abs(m_lLinMotorDVel.Z)) m_lLinMotorDVel.Z = m_linearMotorDirection.Z;
3226 } // else let the motor decay on its own
3227
3228 private void UpdateAngDecay()
3229 {
3230 if (Math.Abs(m_angularMotorDirection.X) > Math.Abs(m_angularMotorDVel.X)) m_angularMotorDVel.X = m_angularMotorDirection.X;
3231 if (Math.Abs(m_angularMotorDirection.Y) > Math.Abs(m_angularMotorDVel.Y)) m_angularMotorDVel.Y = m_angularMotorDirection.Y;
3232 if (Math.Abs(m_angularMotorDirection.Z) > Math.Abs(m_angularMotorDVel.Z)) m_angularMotorDVel.Z = m_angularMotorDirection.Z;
3233 } // else let the motor decay on its own
3234
3235 public void Move(float timestep)
3236 {
3237 float fx = 0;
3238 float fy = 0;
3239 float fz = 0;
3240
3241 frcount++; // used to limit debug comment output
3242 if (frcount > 100)
3243 frcount = 0;
3244
3245 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims.
3246 {
3247
3248// Old public void UpdatePositionAndVelocity(), more accuratley calculated here
3249 // Vector3 pv = Vector3.Zero; // what was this for?
3250 bool lastZeroFlag = _zeroFlag; // was it stopped
3251 // if (Body != (IntPtr)0) // FIXME -> or if it is a joint
3252 // {
3253 d.Vector3 vec = d.BodyGetPosition(Body);
3254 d.Quaternion ori = d.BodyGetQuaternion(Body);
3255 d.Vector3 vel = d.BodyGetLinearVel(Body);
3256 d.Vector3 rotvel = d.BodyGetAngularVel(Body);
3257 d.Vector3 torque = d.BodyGetTorque(Body);
3258 _torque = new Vector3(torque.X, torque.Y, torque.Z);
3259 Vector3 l_position = Vector3.Zero;
3260 Quaternion l_orientation = Quaternion.Identity;
3261
3262 // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!)
3263 //if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
3264 //if (vec.Y < 0.0f) { vec.Y = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
3265 //if (vec.X > 255.95f) { vec.X = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
3266 //if (vec.Y > 255.95f) { vec.Y = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); }
3267
3268 m_lastposition = _position;
3269 m_lastorientation = _orientation;
3270
3271 l_position.X = vec.X;
3272 l_position.Y = vec.Y;
3273 l_position.Z = vec.Z;
3274 l_orientation.X = ori.X;
3275 l_orientation.Y = ori.Y;
3276 l_orientation.Z = ori.Z;
3277 l_orientation.W = ori.W;
3278//Console.WriteLine("Move {0} at {1}", m_primName, l_position);
3279
3280
3281 if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) ||
3282 l_position.X < 0f ||
3283 l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) ||
3284 l_position.Y < 0f)
3285 {
3286 //base.RaiseOutOfBounds(l_position);
3287
3288 if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)
3289 {
3290 _position = l_position;
3291 //_parent_scene.remActivePrim(this);
3292 if (_parent == null)
3293 base.RequestPhysicsterseUpdate();
3294 return;
3295 }
3296 else
3297 {
3298 if (_parent == null)
3299 base.RaiseOutOfBounds(l_position);
3300 return;
3301 }
3302 }
3303
3304 if (l_position.Z < 0)
3305 {
3306 // This is so prim that get lost underground don't fall forever and suck up
3307 //
3308 // Sim resources and memory.
3309 // Disables the prim's movement physics....
3310 // It's a hack and will generate a console message if it fails.
3311
3312 //IsPhysical = false;
3313 if (_parent == null)
3314 base.RaiseOutOfBounds(_position);
3315
3316 _acceleration.X = 0;
3317 _acceleration.Y = 0;
3318 _acceleration.Z = 0;
3319
3320 _velocity.X = 0;
3321 _velocity.Y = 0;
3322 _velocity.Z = 0;
3323 m_rotationalVelocity.X = 0;
3324 m_rotationalVelocity.Y = 0;
3325 m_rotationalVelocity.Z = 0;
3326
3327 if (_parent == null)
3328 base.RequestPhysicsterseUpdate();
3329
3330 m_throttleUpdates = false;
3331 throttleCounter = 0;
3332 _zeroFlag = true;
3333 //outofBounds = true;
3334 }
3335
3336 //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation));
3337//Console.WriteLine("Adiff " + m_primName + " = " + Adiff);
3338 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
3339 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
3340 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
3341// && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01))
3342 && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large
3343 {
3344 _zeroFlag = true;
3345//Console.WriteLine("ZFT 2");
3346 m_throttleUpdates = false;
3347 }
3348 else
3349 {
3350 //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
3351 _zeroFlag = false;
3352 m_lastUpdateSent = false;
3353 //m_throttleUpdates = false;
3354 }
3355
3356 if (_zeroFlag)
3357 { // Its stopped
3358 _velocity.X = 0.0f;
3359 _velocity.Y = 0.0f;
3360 _velocity.Z = 0.0f;
3361
3362 _acceleration.X = 0;
3363 _acceleration.Y = 0;
3364 _acceleration.Z = 0;
3365
3366 //_orientation.w = 0f;
3367 //_orientation.X = 0f;
3368 //_orientation.Y = 0f;
3369 //_orientation.Z = 0f;
3370 m_rotationalVelocity.X = 0;
3371 m_rotationalVelocity.Y = 0;
3372 m_rotationalVelocity.Z = 0;
3373 if (!m_lastUpdateSent)
3374 {
3375 m_throttleUpdates = false;
3376 throttleCounter = 0;
3377 // m_rotationalVelocity = pv; What was this for?
3378
3379 if (_parent == null)
3380 {
3381 base.RequestPhysicsterseUpdate();
3382 }
3383
3384 m_lastUpdateSent = true;
3385 }
3386 }
3387 else
3388 { // Its moving
3389 if (lastZeroFlag != _zeroFlag)
3390 {
3391 if (_parent == null)
3392 {
3393 base.RequestPhysicsterseUpdate();
3394 }
3395 }
3396
3397 m_lastVelocity = _velocity;
3398
3399 _position = l_position;
3400
3401 _velocity.X = vel.X;
3402 _velocity.Y = vel.Y;
3403 _velocity.Z = vel.Z;
3404// Why 2 calcs???
3405// _acceleration = ((_velocity - m_lastVelocity) / 0.1f);
3406// _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f,
3407// _velocity.Y - m_lastVelocity.Y / 0.1f,
3408// _velocity.Z - m_lastVelocity.Z / 0.1f);
3409
3410 _acceleration = ((_velocity - m_lastVelocity) / timestep);
3411
3412 _orientation.X = ori.X;
3413 _orientation.Y = ori.Y;
3414 _orientation.Z = ori.Z;
3415 _orientation.W = ori.W;
3416 m_lastUpdateSent = false;
3417 if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
3418 {
3419 if (_parent == null)
3420 {
3421 base.RequestPhysicsterseUpdate();
3422 }
3423 }
3424 else
3425 {
3426 throttleCounter++;
3427 }
3428 }
3429 m_lastposition = l_position;
3430
3431 /// End of old UpdatePositionAndVelocity insert
3432
3433//if (!Acceleration.ApproxEquals(Vector3.Zero, 0.01f)) Console.WriteLine("Move " + m_primName + " Accel=" + Acceleration);
3434//if(frcount == 0) Console.WriteLine("Move " + m_primName + " VTyp " + m_type +
3435 // " usePID=" + m_usePID + " seHover=" + m_useHoverPID + " useAPID=" + m_useAPID);
3436 if (m_type != Vehicle.TYPE_NONE)
3437 {
3438 // get body attitude
3439 d.Quaternion rot = d.BodyGetQuaternion(Body);
3440 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
3441 Quaternion irotq = Quaternion.Inverse(rotq);
3442
3443 // VEHICLE Linear Motion
3444 d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame
3445 Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z);
3446 m_lLinObjectVel = vel_now * irotq;
3447
3448 if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate
3449 {
3450 if ( Vector3.Mag(m_lLinMotorDVel) < 1.0f)
3451 {
3452 float decayfactor = m_linearMotorDecayTimescale/timestep;
3453 Vector3 decayAmount = (m_lLinMotorDVel/decayfactor);
3454 m_lLinMotorDVel -= decayAmount;
3455 }
3456 else
3457 {
3458 float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale)));
3459 Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * timestep;
3460 m_lLinMotorDVel -= decel;
3461 }
3462 if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
3463 {
3464 m_lLinMotorDVel = Vector3.Zero;
3465 }
3466 else
3467 {
3468 if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X;
3469 if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y;
3470 if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z;
3471 }
3472 } // end linear motor decay
3473
3474 if ( (! m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) )
3475 {
3476 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
3477 if (m_linearMotorTimescale < 300.0f)
3478 {
3479 Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel;
3480 float linfactor = m_linearMotorTimescale/timestep;
3481 Vector3 attackAmount = (attack_error/linfactor) * 1.3f;
3482 m_lLinObjectVel += attackAmount;
3483 }
3484 if (m_linearFrictionTimescale.X < 300.0f)
3485 {
3486 float fricfactor = m_linearFrictionTimescale.X / timestep;
3487 float fricX = m_lLinObjectVel.X / fricfactor;
3488 m_lLinObjectVel.X -= fricX;
3489 }
3490 if (m_linearFrictionTimescale.Y < 300.0f)
3491 {
3492 float fricfactor = m_linearFrictionTimescale.Y / timestep;
3493 float fricY = m_lLinObjectVel.Y / fricfactor;
3494 m_lLinObjectVel.Y -= fricY;
3495 }
3496 if (m_linearFrictionTimescale.Z < 300.0f)
3497 {
3498 float fricfactor = m_linearFrictionTimescale.Z / timestep;
3499//if(frcount == 0) Console.WriteLine("Zfric={0}", fricfactor);
3500 float fricZ = m_lLinObjectVel.Z / fricfactor;
3501 m_lLinObjectVel.Z -= fricZ;
3502 }
3503 }
3504 m_wLinObjectVel = m_lLinObjectVel * rotq;
3505
3506 // Gravity and Buoyancy
3507 Vector3 grav = Vector3.Zero;
3508 if(m_VehicleBuoyancy < 1.0f)
3509 {
3510 // There is some gravity, make a gravity force vector
3511 // that is applied after object velocity.
3512 d.Mass objMass;
3513 d.BodyGetMass(Body, out objMass);
3514 // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
3515 grav.Z = _parent_scene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force
3516 } // else its 1.0, no gravity.
3517
3518 // Hovering
3519 if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
3520 {
3521 // We should hover, get the target height
3522 d.Vector3 pos = d.BodyGetPosition(Body);
3523 if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY)
3524 {
3525 m_VhoverTargetHeight = _parent_scene.GetWaterLevel() + m_VhoverHeight;
3526 }
3527 else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY)
3528 {
3529 m_VhoverTargetHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
3530 }
3531 else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT)
3532 {
3533 m_VhoverTargetHeight = m_VhoverHeight;
3534 }
3535
3536 if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY)
3537 {
3538 // If body is aready heigher, use its height as target height
3539 if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
3540 }
3541
3542// m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
3543// m_VhoverTimescale = 0f; // time to acheive height
3544// timestep is time since last frame,in secs
3545 float herr0 = pos.Z - m_VhoverTargetHeight;
3546 // Replace Vertical speed with correction figure if significant
3547 if(Math.Abs(herr0) > 0.01f )
3548 {
3549 //? d.Mass objMass;
3550 //? d.BodyGetMass(Body, out objMass);
3551 m_wLinObjectVel.Z = - ( (herr0 * timestep * 50.0f) / m_VhoverTimescale);
3552 //KF: m_VhoverEfficiency is not yet implemented
3553 }
3554 else
3555 {
3556 m_wLinObjectVel.Z = 0f;
3557 }
3558 }
3559 else
3560 { // not hovering, Gravity rules
3561 m_wLinObjectVel.Z = vel_now.Z;
3562 }
3563
3564
3565 // Vehicle Linear Motion done =======================================
3566 // Apply velocity
3567 d.BodySetLinearVel(Body, m_wLinObjectVel.X, m_wLinObjectVel.Y, m_wLinObjectVel.Z);
3568 // apply gravity force
3569 d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
3570//if(frcount == 0) Console.WriteLine("Grav {0}", grav);
3571 // end MoveLinear()
3572
3573
3574 // MoveAngular
3575 /*
3576 private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
3577
3578 private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL
3579 private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL
3580 private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL
3581
3582 private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor
3583 private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body
3584 */
3585//if(frcount == 0) Console.WriteLine("MoveAngular ");
3586
3587 d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body);
3588 Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z);
3589 angObjectVel = angObjectVel * irotq; // ============ Converts to LOCAL rotation
3590
3591//if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel);
3592
3593 // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack.
3594 float atk_decayfactor = 23.0f / (m_angularMotorTimescale * timestep);
3595 m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor;
3596 // Decay Angular Motor 2.
3597 if (m_angularMotorDecayTimescale < 300.0f)
3598 {
3599 if ( Vector3.Mag(m_angularMotorDVel) < 1.0f)
3600 {
3601 float decayfactor = (m_angularMotorDecayTimescale)/timestep;
3602 Vector3 decayAmount = (m_angularMotorDVel/decayfactor);
3603 m_angularMotorDVel -= decayAmount;
3604 }
3605 else
3606 {
3607 Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * timestep / m_angularMotorDecayTimescale;
3608 m_angularMotorDVel -= decel;
3609 }
3610
3611 if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f))
3612 {
3613 m_angularMotorDVel = Vector3.Zero;
3614 }
3615 else
3616 {
3617 if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X;
3618 if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y;
3619 if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z;
3620 }
3621 } // end decay angular motor
3622//if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel);
3623
3624//if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel);
3625
3626 // Vertical attractor section
3627 Vector3 vertattr = Vector3.Zero;
3628
3629 if(m_verticalAttractionTimescale < 300)
3630 {
3631 float VAservo = 1.0f / (m_verticalAttractionTimescale * timestep);
3632 // make a vector pointing up
3633 Vector3 verterr = Vector3.Zero;
3634 verterr.Z = 1.0f;
3635 // rotate it to Body Angle
3636 verterr = verterr * rotq;
3637 // 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.
3638 // 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
3639 // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
3640
3641 if (verterr.Z < 0.0f)
3642 { // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to
3643 // vertical, BUT for some reason a z-rotation is imparted to the object. TBI.
3644//Console.WriteLine("InvertFlip");
3645 verterr.X = 2.0f - verterr.X;
3646 verterr.Y = 2.0f - verterr.Y;
3647 }
3648 verterr *= 0.5f;
3649 // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt)
3650
3651 if ((!angObjectVel.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f))
3652 {
3653 // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
3654 // Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
3655 vertattr.X = verterr.Y;
3656 vertattr.Y = - verterr.X;
3657 vertattr.Z = 0f;
3658//if(frcount == 0) Console.WriteLine("VAerr=" + verterr);
3659
3660 // scaling appears better usingsquare-law
3661 float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
3662 float bounce = 1.0f - damped;
3663 // 0 = crit damp, 1 = bouncy
3664 float oavz = angObjectVel.Z; // retain z velocity
3665 // time-scaled correction, which sums, therefore is bouncy:
3666 angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce;
3667 // damped, good @ < 90:
3668 angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped);
3669 angObjectVel.Z = oavz;
3670//if(frcount == 0) Console.WriteLine("VA+");
3671//Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel);
3672 }
3673 else
3674 {
3675 // else error is very small
3676 angObjectVel.X = 0f;
3677 angObjectVel.Y = 0f;
3678//if(frcount == 0) Console.WriteLine("VA0");
3679 }
3680 } // else vertical attractor is off
3681//if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel);
3682
3683 if ( (! m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (! angObjectVel.ApproxEquals(Vector3.Zero, 0.01f)) )
3684 { // if motor or object have motion
3685 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
3686
3687 if (m_angularMotorTimescale < 300.0f)
3688 {
3689 Vector3 attack_error = m_angularMotorDVel - angObjectVel;
3690 float angfactor = m_angularMotorTimescale/timestep;
3691 Vector3 attackAmount = (attack_error/angfactor);
3692 angObjectVel += attackAmount;
3693//if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount);
3694//if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel);
3695 }
3696
3697 angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep);
3698 angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep);
3699 angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep);
3700 } // else no signif. motion
3701
3702//if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel);
3703 // Bank section tba
3704 // Deflection section tba
3705//if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel);
3706
3707 m_lastAngularVelocity = angObjectVel;
3708//if(frcount == 0) Console.WriteLine("angularLock {0}", m_angularLock);
3709
3710 if (!m_angularLock.ApproxEquals(Vector3.One, 0.003f))
3711 {
3712 if (m_angularLock.X == 0)
3713 m_lastAngularVelocity.X = 0f;
3714 if (m_angularLock.Y == 0)
3715 m_lastAngularVelocity.Y = 0f;
3716 if (m_angularLock.Z == 0)
3717 m_lastAngularVelocity.Z = 0f;
3718 }
3719 // Apply to the body
3720// Vector3 aInc = m_lastAngularVelocity - initavel;
3721//if(frcount == 0) Console.WriteLine("Inc {0}", aInc);
3722 m_lastAngularVelocity = m_lastAngularVelocity * rotq; // ================ Converts to WORLD rotation
3723
3724 d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
3725//if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity);
3726
3727 } // end VEHICLES ####
3728 else
3729 {
3730 if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
3731 // NON-'VEHICLES' are dealt with here
3732 // m_angularlock = <1,1,1> means no lock. a 0 on axis means locked.
3733
3734// NB this may be wrong - may lock global axis! Should be LOCAL axis!
3735 /// Dynamics Angular Lock ========================================================================
3736 if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.One, 0.003f))
3737 {
3738 d.Vector3 avel2 = d.BodyGetAngularVel(Body);
3739 if (m_angularlock.X == 0)
3740 avel2.X = 0;
3741 if (m_angularlock.Y == 0)
3742 avel2.Y = 0;
3743 if (m_angularlock.Z == 0)
3744 avel2.Z = 0;
3745 d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
3746 }
3747
3748
3749 /// Dynamics Buoyancy ===============================================================================
3750 //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle.
3751 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
3752 // NB Prims in ODE are no subject to global gravity
3753 float m_mass = CalculateMass();
3754 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass
3755
3756 if (m_usePID)
3757 {
3758//if(frcount == 0) Console.WriteLine("PID " + m_primName);
3759 // KF - this is for object MoveToTarget.
3760
3761 //if (!d.BodyIsEnabled(Body))
3762 //d.BodySetForce(Body, 0f, 0f, 0f);
3763
3764 // no lock; for now it's only called from within Simulate()
3765
3766 // If the PID Controller isn't active then we set our force
3767 // calculating base velocity to the current position
3768
3769 if ((m_PIDTau < 1) && (m_PIDTau != 0))
3770 {
3771 //PID_G = PID_G / m_PIDTau;
3772 m_PIDTau = 1;
3773 }
3774
3775 if ((PID_G - m_PIDTau) <= 0)
3776 {
3777 PID_G = m_PIDTau + 1;
3778 }
3779 //PidStatus = true;
3780
3781 // PhysicsVector vec = new PhysicsVector();
3782// d.Vector3 vel = d.BodyGetLinearVel(Body);
3783
3784 d.Vector3 pos = d.BodyGetPosition(Body);
3785 _target_velocity =
3786 new Vector3(
3787 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
3788 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
3789 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
3790 );
3791
3792 // if velocity is zero, use position control; otherwise, velocity control
3793
3794 if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
3795 {
3796 // keep track of where we stopped. No more slippin' & slidin'
3797
3798 // We only want to deactivate the PID Controller if we think we want to have our surrogate
3799 // react to the physics scene by moving it's position.
3800 // Avatar to Avatar collisions
3801 // Prim to avatar collisions
3802
3803 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
3804 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
3805 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
3806 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3807 d.BodySetLinearVel(Body, 0, 0, 0);
3808 d.BodyAddForce(Body, 0, 0, fz);
3809 return;
3810 }
3811 else
3812 {
3813 _zeroFlag = false;
3814
3815 // We're flying and colliding with something
3816 fx = ((_target_velocity.X) - vel.X) * (PID_D);
3817 fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
3818
3819 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
3820
3821 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
3822 }
3823 } // end if (m_usePID)
3824
3825 /// Dynamics Hover ===================================================================================
3826 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
3827 if (m_useHoverPID && !m_usePID)
3828 {
3829//Console.WriteLine("Hover " + m_primName);
3830
3831 // If we're using the PID controller, then we have no gravity
3832 fz = (-1 * _parent_scene.gravityz) * m_mass;
3833
3834 // no lock; for now it's only called from within Simulate()
3835
3836 // If the PID Controller isn't active then we set our force
3837 // calculating base velocity to the current position
3838
3839 if ((m_PIDTau < 1))
3840 {
3841 PID_G = PID_G / m_PIDTau;
3842 }
3843
3844 if ((PID_G - m_PIDTau) <= 0)
3845 {
3846 PID_G = m_PIDTau + 1;
3847 }
3848
3849
3850 // Where are we, and where are we headed?
3851 d.Vector3 pos = d.BodyGetPosition(Body);
3852// d.Vector3 vel = d.BodyGetLinearVel(Body);
3853
3854
3855 // Non-Vehicles have a limited set of Hover options.
3856 // determine what our target height really is based on HoverType
3857 switch (m_PIDHoverType)
3858 {
3859 case PIDHoverType.Ground:
3860 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3861 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3862 break;
3863 case PIDHoverType.GroundAndWater:
3864 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3865 m_waterHeight = _parent_scene.GetWaterLevel();
3866 if (m_groundHeight > m_waterHeight)
3867 {
3868 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3869 }
3870 else
3871 {
3872 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
3873 }
3874 break;
3875
3876 } // end switch (m_PIDHoverType)
3877
3878
3879 _target_velocity =
3880 new Vector3(0.0f, 0.0f,
3881 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
3882 );
3883
3884 // if velocity is zero, use position control; otherwise, velocity control
3885
3886 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
3887 {
3888 // keep track of where we stopped. No more slippin' & slidin'
3889
3890 // We only want to deactivate the PID Controller if we think we want to have our surrogate
3891 // react to the physics scene by moving it's position.
3892 // Avatar to Avatar collisions
3893 // Prim to avatar collisions
3894
3895 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
3896 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
3897 d.BodyAddForce(Body, 0, 0, fz);
3898 //KF this prevents furthur motions return;
3899 }
3900 else
3901 {
3902 _zeroFlag = false;
3903
3904 // We're flying and colliding with something
3905 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
3906 }
3907 } // end m_useHoverPID && !m_usePID
3908
3909 /// Dynamics RotLookAt =================================================================================
3910 if (m_useAPID)
3911 {
3912 // RotLookAt, apparently overrides all other rotation sources. Inputs:
3913 // Quaternion m_APIDTarget
3914 // float m_APIDStrength // From SL experiments, this is the time to get there
3915 // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly
3916 // Also in SL the mass of the object has no effect on time to get there.
3917 // Factors:
3918//if(frcount == 0) Console.WriteLine("APID ");
3919 // get present body rotation
3920 float limit = 1.0f;
3921 float scaler = 50f; // adjusts damping time
3922 float RLAservo = 0f;
3923
3924 d.Quaternion rot = d.BodyGetQuaternion(Body);
3925 Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
3926 Quaternion rot_diff = Quaternion.Inverse(rotq) * m_APIDTarget;
3927 float diff_angle;
3928 Vector3 diff_axis;
3929 rot_diff.GetAxisAngle(out diff_axis, out diff_angle);
3930 diff_axis.Normalize();
3931 if(diff_angle > 0.01f) // diff_angle is always +ve
3932 {
3933// PhysicsVector rotforce = new PhysicsVector(diff_axis.X, diff_axis.Y, diff_axis.Z);
3934 Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z);
3935 rotforce = rotforce * rotq;
3936 if(diff_angle > limit) diff_angle = limit; // cap the rotate rate
3937// RLAservo = timestep / m_APIDStrength * m_mass * scaler;
3938 // rotforce = rotforce * RLAservo * diff_angle ;
3939 // d.BodyAddRelTorque(Body, rotforce.X, rotforce.Y, rotforce.Z);
3940 RLAservo = timestep / m_APIDStrength * scaler;
3941 rotforce = rotforce * RLAservo * diff_angle ;
3942
3943 if (m_angularlock.X == 0)
3944 rotforce.X = 0;
3945 if (m_angularlock.Y == 0)
3946 rotforce.Y = 0;
3947 if (m_angularlock.Z == 0)
3948 rotforce.Z = 0;
3949
3950 d.BodySetAngularVel (Body, rotforce.X, rotforce.Y, rotforce.Z);
3951//Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo);
3952 }
3953//if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle);
3954 } // end m_useAPID
3955
3956 /// Dynamics Apply Forces ===================================================================================
3957 fx *= m_mass;
3958 fy *= m_mass;
3959 //fz *= m_mass;
3960
3961 fx += m_force.X;
3962 fy += m_force.Y;
3963 fz += m_force.Z;
3964
3965 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
3966 if (fx != 0 || fy != 0 || fz != 0)
3967 {
3968 //m_taintdisable = true;
3969 //base.RaiseOutOfBounds(Position);
3970 //d.BodySetLinearVel(Body, fx, fy, 0f);
3971 if (!d.BodyIsEnabled(Body))
3972 {
3973 // A physical body at rest on a surface will auto-disable after a while,
3974 // this appears to re-enable it incase the surface it is upon vanishes,
3975 // and the body should fall again.
3976 d.BodySetLinearVel(Body, 0f, 0f, 0f);
3977 d.BodySetForce(Body, 0, 0, 0);
3978 enableBodySoft();
3979 }
3980
3981 // 35x10 = 350n times the mass per second applied maximum.
3982 float nmax = 35f * m_mass;
3983 float nmin = -35f * m_mass;
3984
3985
3986 if (fx > nmax)
3987 fx = nmax;
3988 if (fx < nmin)
3989 fx = nmin;
3990 if (fy > nmax)
3991 fy = nmax;
3992 if (fy < nmin)
3993 fy = nmin;
3994 d.BodyAddForce(Body, fx, fy, fz);
3995//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
3996 }
3997 }
3998 }
3999 else
4000 { // is not physical, or is not a body or is selected
4001 // from old UpdatePositionAndVelocity, ... Not a body.. so Make sure the client isn't interpolating
4002 _velocity.X = 0;
4003 _velocity.Y = 0;
4004 _velocity.Z = 0;
4005
4006 _acceleration.X = 0;
4007 _acceleration.Y = 0;
4008 _acceleration.Z = 0;
4009
4010 m_rotationalVelocity.X = 0;
4011 m_rotationalVelocity.Y = 0;
4012 m_rotationalVelocity.Z = 0;
4013 _zeroFlag = true;
4014 return;
4015 }
4016 } // end Move()
4017 } // end class
3322} 4018}
generated by cgit v1.1 at 2024-12-14 12:53:33 +0000