diff options
author | Robert Adams | 2015-09-08 06:15:46 -0700 |
---|---|---|
committer | Robert Adams | 2015-09-08 06:15:46 -0700 |
commit | 4dd17c4117ea413fb0c4418511956cb3abfe258c (patch) | |
tree | 6f31a583610f8bf074d1f0b4e7ab6ccef37dec9a /OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | |
parent | Merge of ubitworkvarnew with opensim/master as of 20150905. (diff) | |
download | opensim-SC_OLD-4dd17c4117ea413fb0c4418511956cb3abfe258c.zip opensim-SC_OLD-4dd17c4117ea413fb0c4418511956cb3abfe258c.tar.gz opensim-SC_OLD-4dd17c4117ea413fb0c4418511956cb3abfe258c.tar.bz2 opensim-SC_OLD-4dd17c4117ea413fb0c4418511956cb3abfe258c.tar.xz |
More 'everything is a module' merging.
Have most of UbitOde converted.
There are compile errors in OpenSimBase as the new modules stuff is not all there.
Removed ChOdePlugin as it's connection to OdePlugin was tangled.
Diffstat (limited to '')
-rw-r--r-- | OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs | 4124 |
1 files changed, 0 insertions, 4124 deletions
diff --git a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs deleted file mode 100644 index b283152..0000000 --- a/OpenSim/Region/Physics/ChOdePlugin/ODEPrim.cs +++ /dev/null | |||
@@ -1,4124 +0,0 @@ | |||
1 | /* Copyright (c) Contributors, http://opensimulator.org/ | ||
2 | * See CONTRIBUTORS.TXT for a full list of copyright holders. | ||
3 | * Redistribution and use in source and binary forms, with or without | ||
4 | * modification, are permitted provided that the following conditions are met: | ||
5 | * * Redistributions of source code must retain the above copyright | ||
6 | * notice, this list of conditions and the following disclaimer. | ||
7 | * * Redistributions in binary form must reproduce the above copyright | ||
8 | * notice, this list of conditions and the following disclaimer in the | ||
9 | * documentation and/or other materials provided with the distribution. | ||
10 | * * Neither the name of the OpenSimulator Project nor the | ||
11 | * names of its contributors may be used to endorse or promote products | ||
12 | * derived from this software without specific prior written permission. | ||
13 | * | ||
14 | * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY | ||
15 | * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED | ||
16 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE | ||
17 | * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY | ||
18 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES | ||
19 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
20 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND | ||
21 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
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. | ||
24 | * | ||
25 | * Revised March 5th 2010 by Kitto Flora. ODEDynamics.cs | ||
26 | * Ubit 2012 | ||
27 | * rolled into ODEPrim.cs | ||
28 | */ | ||
29 | |||
30 | using System; | ||
31 | using System.IO; | ||
32 | using System.Collections.Generic; | ||
33 | using System.Reflection; | ||
34 | using System.Runtime.InteropServices; | ||
35 | using System.Threading; | ||
36 | using log4net; | ||
37 | using OpenMetaverse; | ||
38 | using Ode.NET; | ||
39 | using OpenSim.Framework; | ||
40 | using OpenSim.Region.Physics.Manager; | ||
41 | |||
42 | namespace OpenSim.Region.Physics.OdePlugin | ||
43 | { | ||
44 | /// <summary> | ||
45 | /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves. | ||
46 | /// </summary> | ||
47 | |||
48 | public class OdePrim : PhysicsActor | ||
49 | { | ||
50 | private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); | ||
51 | |||
52 | public class SerialControl | ||
53 | { | ||
54 | public object alock = new object(); | ||
55 | public byte[] data = new byte[0]; | ||
56 | } | ||
57 | private Vector3 _position; | ||
58 | private Vector3 _velocity; | ||
59 | private Vector3 _torque; | ||
60 | private Vector3 m_lastVelocity; | ||
61 | private Vector3 m_lastposition; | ||
62 | private Quaternion m_lastorientation = new Quaternion(); | ||
63 | private Vector3 m_rotationalVelocity; | ||
64 | private Vector3 _size; | ||
65 | private Vector3 _acceleration; | ||
66 | // private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f); | ||
67 | private Quaternion _orientation; | ||
68 | private Vector3 m_taintposition; | ||
69 | private Vector3 m_taintsize; | ||
70 | private Vector3 m_taintVelocity; | ||
71 | private Vector3 m_taintTorque; | ||
72 | private Quaternion m_taintrot; | ||
73 | private Vector3 m_rotateEnable = Vector3.One; // Current setting | ||
74 | private Vector3 m_rotateEnableRequest = Vector3.One; // Request from LSL | ||
75 | private bool m_rotateEnableUpdate = false; | ||
76 | private Vector3 m_lockX; | ||
77 | private Vector3 m_lockY; | ||
78 | private Vector3 m_lockZ; | ||
79 | private IntPtr Amotor = IntPtr.Zero; | ||
80 | private IntPtr AmotorX = IntPtr.Zero; | ||
81 | private IntPtr AmotorY = IntPtr.Zero; | ||
82 | private IntPtr AmotorZ = IntPtr.Zero; | ||
83 | |||
84 | private Vector3 m_PIDTarget; | ||
85 | private float m_PIDTau; | ||
86 | private float PID_D = 35f; | ||
87 | private float PID_G = 25f; | ||
88 | private bool m_usePID = false; | ||
89 | |||
90 | private Quaternion m_APIDTarget = new Quaternion(); | ||
91 | private float m_APIDStrength = 0.5f; | ||
92 | private float m_APIDDamping = 0.5f; | ||
93 | private bool m_useAPID = false; | ||
94 | private float m_APIDdamper = 1.0f; | ||
95 | |||
96 | // These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), | ||
97 | // do not confuse with VEHICLE HOVER | ||
98 | |||
99 | private float m_PIDHoverHeight; | ||
100 | private float m_PIDHoverTau; | ||
101 | private bool m_useHoverPID; | ||
102 | private PIDHoverType m_PIDHoverType = PIDHoverType.Ground; | ||
103 | private float m_targetHoverHeight; | ||
104 | private float m_groundHeight; | ||
105 | private float m_waterHeight; | ||
106 | private float m_buoyancy; //m_buoyancy set by llSetBuoyancy() | ||
107 | |||
108 | // private float m_tensor = 5f; | ||
109 | private int body_autodisable_frames = 20; | ||
110 | |||
111 | |||
112 | private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom | ||
113 | | CollisionCategories.Space | ||
114 | | CollisionCategories.Body | ||
115 | | CollisionCategories.Character | ||
116 | ); | ||
117 | private bool m_taintshape; | ||
118 | private bool m_taintPhysics; | ||
119 | private bool m_collidesLand = true; | ||
120 | private bool m_collidesWater; | ||
121 | // public bool m_returnCollisions; | ||
122 | |||
123 | // Default we're a Geometry | ||
124 | private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); | ||
125 | |||
126 | // Default, Collide with Other Geometries, spaces and Bodies | ||
127 | private CollisionCategories m_collisionFlags = m_default_collisionFlags; | ||
128 | |||
129 | public bool m_taintremove; | ||
130 | public bool m_taintdisable; | ||
131 | public bool m_disabled; | ||
132 | public bool m_taintadd; | ||
133 | public bool m_taintselected; | ||
134 | public bool m_taintphantom; | ||
135 | public bool m_taintCollidesWater; | ||
136 | |||
137 | public uint m_localID; | ||
138 | |||
139 | //public GCHandle gc; | ||
140 | private CollisionLocker ode; | ||
141 | |||
142 | private bool m_meshfailed = false; | ||
143 | private bool m_taintforce = false; | ||
144 | private bool m_taintaddangularforce = false; | ||
145 | private Vector3 m_force; | ||
146 | private List<Vector3> m_forcelist = new List<Vector3>(); | ||
147 | private List<Vector3> m_angularforcelist = new List<Vector3>(); | ||
148 | |||
149 | private IMesh _mesh; | ||
150 | private PrimitiveBaseShape _pbs; | ||
151 | private OdeScene _parent_scene; | ||
152 | public IntPtr m_targetSpace = IntPtr.Zero; | ||
153 | public IntPtr prim_geom; | ||
154 | // public IntPtr prev_geom; | ||
155 | public IntPtr _triMeshData; | ||
156 | |||
157 | private IntPtr _linkJointGroup = IntPtr.Zero; | ||
158 | private PhysicsActor _parent; | ||
159 | private PhysicsActor m_taintparent; | ||
160 | |||
161 | private List<OdePrim> childrenPrim = new List<OdePrim>(); | ||
162 | |||
163 | private bool iscolliding; | ||
164 | private bool m_isphysical; | ||
165 | private bool m_isphantom; | ||
166 | private bool m_isSelected; | ||
167 | |||
168 | private bool m_NoColide; // for now only for internal use for bad meshs | ||
169 | |||
170 | internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively | ||
171 | |||
172 | private bool m_throttleUpdates; | ||
173 | private int throttleCounter; | ||
174 | public int m_interpenetrationcount; | ||
175 | public float m_collisionscore; | ||
176 | // public int m_roundsUnderMotionThreshold; | ||
177 | // private int m_crossingfailures; | ||
178 | |||
179 | public bool m_outofBounds; | ||
180 | private float m_density = 10.000006836f; // Aluminum g/cm3; | ||
181 | |||
182 | private float m_primMass = 10.000006836f; // Aluminum g/cm3; | ||
183 | |||
184 | private byte m_shapetype; | ||
185 | private byte m_taintshapetype; | ||
186 | |||
187 | public bool _zeroFlag; // if body has been stopped | ||
188 | private bool m_lastUpdateSent; | ||
189 | |||
190 | public IntPtr Body = IntPtr.Zero; | ||
191 | public String m_primName; | ||
192 | private Vector3 _target_velocity; | ||
193 | public d.Mass pMass; | ||
194 | |||
195 | public int m_eventsubscription; | ||
196 | private CollisionEventUpdate CollisionEventsThisFrame; | ||
197 | |||
198 | private IntPtr m_linkJoint = IntPtr.Zero; | ||
199 | |||
200 | public volatile bool childPrim; | ||
201 | |||
202 | internal int m_material = (int)Material.Wood; | ||
203 | |||
204 | private IntPtr m_body = IntPtr.Zero; | ||
205 | |||
206 | // Vehicle properties ============================================================================================ | ||
207 | private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind | ||
208 | // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier | ||
209 | private VehicleFlag m_flags = (VehicleFlag)0; // Bit settings: | ||
210 | // HOVER_TERRAIN_ONLY | ||
211 | // HOVER_GLOBAL_HEIGHT | ||
212 | // NO_DEFLECTION_UP | ||
213 | // HOVER_WATER_ONLY | ||
214 | // HOVER_UP_ONLY | ||
215 | // LIMIT_MOTOR_UP | ||
216 | // LIMIT_ROLL_ONLY | ||
217 | |||
218 | // Linear properties | ||
219 | private Vector3 m_linearMotorDirection = Vector3.Zero; // (was m_linearMotorDirectionLASTSET) the (local) Velocity | ||
220 | //requested by LSL | ||
221 | private float m_linearMotorTimescale = 0; // Motor Attack rate set by LSL | ||
222 | private float m_linearMotorDecayTimescale = 0; // Motor Decay rate set by LSL | ||
223 | private Vector3 m_linearFrictionTimescale = Vector3.Zero; // General Friction set by LSL | ||
224 | |||
225 | private Vector3 m_lLinMotorDVel = Vector3.Zero; // decayed motor | ||
226 | private Vector3 m_lLinObjectVel = Vector3.Zero; // local frame object velocity | ||
227 | private Vector3 m_wLinObjectVel = Vector3.Zero; // world frame object velocity | ||
228 | |||
229 | //Angular properties | ||
230 | private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor | ||
231 | |||
232 | private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL | ||
233 | private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL | ||
234 | private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL | ||
235 | |||
236 | private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor | ||
237 | // private Vector3 m_angObjectVel = Vector3.Zero; // current body angular velocity | ||
238 | private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body | ||
239 | |||
240 | //Deflection properties | ||
241 | // private float m_angularDeflectionEfficiency = 0; | ||
242 | // private float m_angularDeflectionTimescale = 0; | ||
243 | // private float m_linearDeflectionEfficiency = 0; | ||
244 | // private float m_linearDeflectionTimescale = 0; | ||
245 | |||
246 | //Banking properties | ||
247 | // private float m_bankingEfficiency = 0; | ||
248 | // private float m_bankingMix = 0; | ||
249 | // private float m_bankingTimescale = 0; | ||
250 | |||
251 | //Hover and Buoyancy properties | ||
252 | private float m_VhoverHeight = 0f; | ||
253 | // private float m_VhoverEfficiency = 0f; | ||
254 | private float m_VhoverTimescale = 0f; | ||
255 | private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height | ||
256 | private float m_VehicleBuoyancy = 0f; // Set by VEHICLE_BUOYANCY, for a vehicle. | ||
257 | // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) | ||
258 | // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. | ||
259 | // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. | ||
260 | |||
261 | //Attractor properties | ||
262 | private float m_verticalAttractionEfficiency = 1.0f; // damped | ||
263 | private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor. | ||
264 | |||
265 | // SerialControl m_taintserial = null; | ||
266 | object m_taintvehicledata = null; | ||
267 | |||
268 | public void DoSetVehicle() | ||
269 | { | ||
270 | VehicleData vd = (VehicleData)m_taintvehicledata; | ||
271 | |||
272 | m_type = vd.m_type; | ||
273 | m_flags = vd.m_flags; | ||
274 | |||
275 | // Linear properties | ||
276 | m_linearMotorDirection = vd.m_linearMotorDirection; | ||
277 | m_linearFrictionTimescale = vd.m_linearFrictionTimescale; | ||
278 | m_linearMotorDecayTimescale = vd.m_linearMotorDecayTimescale; | ||
279 | m_linearMotorTimescale = vd.m_linearMotorTimescale; | ||
280 | // m_linearMotorOffset = vd.m_linearMotorOffset; | ||
281 | |||
282 | //Angular properties | ||
283 | m_angularMotorDirection = vd.m_angularMotorDirection; | ||
284 | m_angularMotorTimescale = vd.m_angularMotorTimescale; | ||
285 | m_angularMotorDecayTimescale = vd.m_angularMotorDecayTimescale; | ||
286 | m_angularFrictionTimescale = vd.m_angularFrictionTimescale; | ||
287 | |||
288 | //Deflection properties | ||
289 | // m_angularDeflectionEfficiency = vd.m_angularDeflectionEfficiency; | ||
290 | // m_angularDeflectionTimescale = vd.m_angularDeflectionTimescale; | ||
291 | // m_linearDeflectionEfficiency = vd.m_linearDeflectionEfficiency; | ||
292 | // m_linearDeflectionTimescale = vd.m_linearDeflectionTimescale; | ||
293 | |||
294 | //Banking properties | ||
295 | // m_bankingEfficiency = vd.m_bankingEfficiency; | ||
296 | // m_bankingMix = vd.m_bankingMix; | ||
297 | // m_bankingTimescale = vd.m_bankingTimescale; | ||
298 | |||
299 | //Hover and Buoyancy properties | ||
300 | m_VhoverHeight = vd.m_VhoverHeight; | ||
301 | // m_VhoverEfficiency = vd.m_VhoverEfficiency; | ||
302 | m_VhoverTimescale = vd.m_VhoverTimescale; | ||
303 | m_VehicleBuoyancy = vd.m_VehicleBuoyancy; | ||
304 | |||
305 | //Attractor properties | ||
306 | m_verticalAttractionEfficiency = vd.m_verticalAttractionEfficiency; | ||
307 | m_verticalAttractionTimescale = vd.m_verticalAttractionTimescale; | ||
308 | |||
309 | // Axis | ||
310 | // m_referenceFrame = vd.m_referenceFrame; | ||
311 | |||
312 | |||
313 | m_taintvehicledata = null; | ||
314 | } | ||
315 | |||
316 | public override void SetVehicle(object vdata) | ||
317 | { | ||
318 | m_taintvehicledata = vdata; | ||
319 | _parent_scene.AddPhysicsActorTaint(this); | ||
320 | } | ||
321 | |||
322 | public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, | ||
323 | Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical, | ||
324 | bool pisPhantom,byte shapetype, CollisionLocker dode, uint localid) | ||
325 | { | ||
326 | m_localID = localid; | ||
327 | ode = dode; | ||
328 | if (!pos.IsFinite()) | ||
329 | { | ||
330 | pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), | ||
331 | parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); | ||
332 | m_log.Warn("[PHYSICS]: Got nonFinite Object create Position"); | ||
333 | } | ||
334 | |||
335 | _position = pos; | ||
336 | m_taintposition = pos; | ||
337 | PID_D = parent_scene.bodyPIDD; | ||
338 | PID_G = parent_scene.bodyPIDG; | ||
339 | m_density = parent_scene.geomDefaultDensity; | ||
340 | // m_tensor = parent_scene.bodyMotorJointMaxforceTensor; | ||
341 | body_autodisable_frames = parent_scene.bodyFramesAutoDisable; | ||
342 | |||
343 | prim_geom = IntPtr.Zero; | ||
344 | // prev_geom = IntPtr.Zero; | ||
345 | |||
346 | if (!pos.IsFinite()) | ||
347 | { | ||
348 | size = new Vector3(0.5f, 0.5f, 0.5f); | ||
349 | m_log.Warn("[PHYSICS]: Got nonFinite Object create Size"); | ||
350 | } | ||
351 | |||
352 | if (size.X <= 0) size.X = 0.01f; | ||
353 | if (size.Y <= 0) size.Y = 0.01f; | ||
354 | if (size.Z <= 0) size.Z = 0.01f; | ||
355 | |||
356 | _size = size; | ||
357 | m_taintsize = _size; | ||
358 | |||
359 | if (!QuaternionIsFinite(rotation)) | ||
360 | { | ||
361 | rotation = Quaternion.Identity; | ||
362 | m_log.Warn("[PHYSICS]: Got nonFinite Object create Rotation"); | ||
363 | } | ||
364 | |||
365 | _orientation = rotation; | ||
366 | m_taintrot = _orientation; | ||
367 | _mesh = mesh; | ||
368 | _pbs = pbs; | ||
369 | m_shapetype = shapetype; | ||
370 | m_taintshapetype = shapetype; | ||
371 | |||
372 | _parent_scene = parent_scene; | ||
373 | m_targetSpace = (IntPtr)0; | ||
374 | |||
375 | // if (pos.Z < 0) | ||
376 | if (pos.Z < parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y)) | ||
377 | m_isphysical = false; | ||
378 | else | ||
379 | { | ||
380 | m_isphysical = pisPhysical; | ||
381 | // If we're physical, we need to be in the master space for now. | ||
382 | // linksets *should* be in a space together.. but are not currently | ||
383 | if (m_isphysical) | ||
384 | m_targetSpace = _parent_scene.space; | ||
385 | } | ||
386 | |||
387 | m_isphantom = pisPhantom; | ||
388 | m_taintphantom = pisPhantom; | ||
389 | |||
390 | _triMeshData = IntPtr.Zero; | ||
391 | m_NoColide = false; | ||
392 | |||
393 | // m_taintserial = null; | ||
394 | m_primName = primName; | ||
395 | m_taintadd = true; | ||
396 | _parent_scene.AddPhysicsActorTaint(this); | ||
397 | // don't do .add() here; old geoms get recycled with the same hash | ||
398 | } | ||
399 | |||
400 | public override int PhysicsActorType | ||
401 | { | ||
402 | get { return (int)ActorTypes.Prim; } | ||
403 | set { return; } | ||
404 | } | ||
405 | |||
406 | public override bool SetAlwaysRun | ||
407 | { | ||
408 | get { return false; } | ||
409 | set { return; } | ||
410 | } | ||
411 | |||
412 | public override uint LocalID | ||
413 | { | ||
414 | set | ||
415 | { | ||
416 | //m_log.Info("[PHYSICS]: Setting TrackerID: " + value); | ||
417 | m_localID = value; | ||
418 | } | ||
419 | } | ||
420 | |||
421 | public override bool Grabbed | ||
422 | { | ||
423 | set { return; } | ||
424 | } | ||
425 | |||
426 | public override bool Selected | ||
427 | { | ||
428 | set | ||
429 | { | ||
430 | //Console.WriteLine("Sel {0} {1} {2}", m_primName, value, m_isphysical); | ||
431 | // This only makes the object not collidable if the object | ||
432 | // is physical or the object is modified somehow *IN THE FUTURE* | ||
433 | // without this, if an avatar selects prim, they can walk right | ||
434 | // through it while it's selected | ||
435 | m_collisionscore = 0; | ||
436 | if ((m_isphysical && !_zeroFlag) || !value) | ||
437 | { | ||
438 | m_taintselected = value; | ||
439 | _parent_scene.AddPhysicsActorTaint(this); | ||
440 | } | ||
441 | else | ||
442 | { | ||
443 | m_taintselected = value; | ||
444 | m_isSelected = value; | ||
445 | } | ||
446 | if (m_isSelected) disableBodySoft(); | ||
447 | } | ||
448 | } | ||
449 | |||
450 | public override bool IsPhysical | ||
451 | { | ||
452 | get { return m_isphysical; } | ||
453 | set | ||
454 | { | ||
455 | m_isphysical = value; | ||
456 | if (!m_isphysical) | ||
457 | { // Zero the remembered last velocity | ||
458 | m_lastVelocity = Vector3.Zero; | ||
459 | if (m_type != Vehicle.TYPE_NONE) Halt(); | ||
460 | } | ||
461 | } | ||
462 | } | ||
463 | |||
464 | public override bool IsVolumeDtc | ||
465 | { | ||
466 | set { return; } | ||
467 | get { return m_isVolumeDetect; } | ||
468 | |||
469 | } | ||
470 | |||
471 | public override bool Phantom | ||
472 | { | ||
473 | get { return m_isphantom; } | ||
474 | set | ||
475 | { | ||
476 | m_isphantom = value; | ||
477 | } | ||
478 | } | ||
479 | |||
480 | public void setPrimForRemoval() | ||
481 | { | ||
482 | m_taintremove = true; | ||
483 | } | ||
484 | |||
485 | public override bool Flying | ||
486 | { | ||
487 | // no flying prims for you | ||
488 | get { return false; } | ||
489 | set { } | ||
490 | } | ||
491 | |||
492 | public override bool IsColliding | ||
493 | { | ||
494 | get { return iscolliding; } | ||
495 | set { iscolliding = value; } | ||
496 | } | ||
497 | |||
498 | public override bool CollidingGround | ||
499 | { | ||
500 | get { return false; } | ||
501 | set { return; } | ||
502 | } | ||
503 | |||
504 | public override bool CollidingObj | ||
505 | { | ||
506 | get { return false; } | ||
507 | set { return; } | ||
508 | } | ||
509 | |||
510 | public override bool ThrottleUpdates | ||
511 | { | ||
512 | get { return m_throttleUpdates; } | ||
513 | set { m_throttleUpdates = value; } | ||
514 | } | ||
515 | |||
516 | public override bool Stopped | ||
517 | { | ||
518 | get { return _zeroFlag; } | ||
519 | } | ||
520 | |||
521 | public override Vector3 Position | ||
522 | { | ||
523 | get { return _position; } | ||
524 | |||
525 | set | ||
526 | { | ||
527 | _position = value; | ||
528 | //m_log.Info("[PHYSICS]: " + _position.ToString()); | ||
529 | } | ||
530 | } | ||
531 | |||
532 | public override Vector3 Size | ||
533 | { | ||
534 | get { return _size; } | ||
535 | set | ||
536 | { | ||
537 | if (value.IsFinite()) | ||
538 | { | ||
539 | _size = value; | ||
540 | } | ||
541 | else | ||
542 | { | ||
543 | m_log.Warn("[PHYSICS]: Got NaN Size on object"); | ||
544 | } | ||
545 | } | ||
546 | } | ||
547 | |||
548 | public override float Mass | ||
549 | { | ||
550 | get | ||
551 | { | ||
552 | CalculateMass(); | ||
553 | return m_primMass; | ||
554 | } | ||
555 | } | ||
556 | |||
557 | public override Vector3 Force | ||
558 | { | ||
559 | //get { return Vector3.Zero; } | ||
560 | get { return m_force; } | ||
561 | set | ||
562 | { | ||
563 | if (value.IsFinite()) | ||
564 | { | ||
565 | m_force = value; | ||
566 | } | ||
567 | else | ||
568 | { | ||
569 | m_log.Warn("[PHYSICS]: NaN in Force Applied to an Object"); | ||
570 | } | ||
571 | } | ||
572 | } | ||
573 | |||
574 | public override int VehicleType | ||
575 | { | ||
576 | get { return (int)m_type; } | ||
577 | set { ProcessTypeChange((Vehicle)value); } | ||
578 | } | ||
579 | |||
580 | public override void VehicleFloatParam(int param, float value) | ||
581 | { | ||
582 | ProcessFloatVehicleParam((Vehicle)param, value); | ||
583 | } | ||
584 | |||
585 | public override void VehicleVectorParam(int param, Vector3 value) | ||
586 | { | ||
587 | ProcessVectorVehicleParam((Vehicle)param, value); | ||
588 | } | ||
589 | |||
590 | public override void VehicleRotationParam(int param, Quaternion rotation) | ||
591 | { | ||
592 | ProcessRotationVehicleParam((Vehicle)param, rotation); | ||
593 | } | ||
594 | |||
595 | public override void VehicleFlags(int param, bool remove) | ||
596 | { | ||
597 | ProcessVehicleFlags(param, remove); | ||
598 | } | ||
599 | |||
600 | public override void SetVolumeDetect(int param) | ||
601 | { | ||
602 | lock (_parent_scene.OdeLock) | ||
603 | { | ||
604 | m_isVolumeDetect = (param != 0); | ||
605 | } | ||
606 | } | ||
607 | |||
608 | |||
609 | public override Vector3 CenterOfMass | ||
610 | { | ||
611 | get { return Vector3.Zero; } | ||
612 | } | ||
613 | |||
614 | public override Vector3 GeometricCenter | ||
615 | { | ||
616 | get { return Vector3.Zero; } | ||
617 | } | ||
618 | |||
619 | public override PrimitiveBaseShape Shape | ||
620 | { | ||
621 | set | ||
622 | { | ||
623 | _pbs = value; | ||
624 | m_taintshape = true; | ||
625 | } | ||
626 | } | ||
627 | |||
628 | public override byte PhysicsShapeType | ||
629 | { | ||
630 | get | ||
631 | { | ||
632 | return m_shapetype; | ||
633 | } | ||
634 | set | ||
635 | { | ||
636 | m_taintshapetype = value; | ||
637 | _parent_scene.AddPhysicsActorTaint(this); | ||
638 | } | ||
639 | } | ||
640 | |||
641 | public override Vector3 Velocity | ||
642 | { | ||
643 | get | ||
644 | { | ||
645 | // Averate previous velocity with the new one so | ||
646 | // client object interpolation works a 'little' better | ||
647 | if (_zeroFlag) | ||
648 | return Vector3.Zero; | ||
649 | |||
650 | Vector3 returnVelocity = Vector3.Zero; | ||
651 | returnVelocity.X = (m_lastVelocity.X + _velocity.X) / 2; | ||
652 | returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) / 2; | ||
653 | returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) / 2; | ||
654 | return returnVelocity; | ||
655 | } | ||
656 | set | ||
657 | { | ||
658 | if (value.IsFinite()) | ||
659 | { | ||
660 | _velocity = value; | ||
661 | if (_velocity.ApproxEquals(Vector3.Zero, 0.001f)) | ||
662 | _acceleration = Vector3.Zero; | ||
663 | |||
664 | m_taintVelocity = value; | ||
665 | _parent_scene.AddPhysicsActorTaint(this); | ||
666 | } | ||
667 | else | ||
668 | { | ||
669 | m_log.Warn("[PHYSICS]: Got NaN Velocity in Object"); | ||
670 | } | ||
671 | |||
672 | } | ||
673 | } | ||
674 | |||
675 | public override Vector3 Torque | ||
676 | { | ||
677 | get | ||
678 | { | ||
679 | if (!m_isphysical || Body == IntPtr.Zero) | ||
680 | return Vector3.Zero; | ||
681 | |||
682 | return _torque; | ||
683 | } | ||
684 | |||
685 | set | ||
686 | { | ||
687 | if (value.IsFinite()) | ||
688 | { | ||
689 | m_taintTorque = value; | ||
690 | _parent_scene.AddPhysicsActorTaint(this); | ||
691 | } | ||
692 | else | ||
693 | { | ||
694 | m_log.Warn("[PHYSICS]: Got NaN Torque in Object"); | ||
695 | } | ||
696 | } | ||
697 | } | ||
698 | |||
699 | public override float CollisionScore | ||
700 | { | ||
701 | get { return m_collisionscore; } | ||
702 | set { m_collisionscore = value; } | ||
703 | } | ||
704 | |||
705 | public override bool Kinematic | ||
706 | { | ||
707 | get { return false; } | ||
708 | set { } | ||
709 | } | ||
710 | |||
711 | public override Quaternion Orientation | ||
712 | { | ||
713 | get { return _orientation; } | ||
714 | set | ||
715 | { | ||
716 | if (QuaternionIsFinite(value)) | ||
717 | { | ||
718 | _orientation = value; | ||
719 | } | ||
720 | else | ||
721 | m_log.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object"); | ||
722 | |||
723 | } | ||
724 | } | ||
725 | |||
726 | public override bool FloatOnWater | ||
727 | { | ||
728 | set | ||
729 | { | ||
730 | m_taintCollidesWater = value; | ||
731 | _parent_scene.AddPhysicsActorTaint(this); | ||
732 | } | ||
733 | } | ||
734 | |||
735 | public override void SetMomentum(Vector3 momentum) | ||
736 | { | ||
737 | } | ||
738 | |||
739 | public override Vector3 PIDTarget | ||
740 | { | ||
741 | set | ||
742 | { | ||
743 | if (value.IsFinite()) | ||
744 | { | ||
745 | m_PIDTarget = value; | ||
746 | } | ||
747 | else | ||
748 | m_log.Warn("[PHYSICS]: Got NaN PIDTarget from Scene on Object"); | ||
749 | } | ||
750 | } | ||
751 | public override bool PIDActive { get { return m_usePID; } set { m_usePID = value; } } | ||
752 | public override float PIDTau { set { m_PIDTau = value; } } | ||
753 | |||
754 | // For RotLookAt | ||
755 | public override Quaternion APIDTarget { set { m_APIDTarget = value; } } | ||
756 | public override bool APIDActive { set { m_useAPID = value; } } | ||
757 | public override float APIDStrength { set { m_APIDStrength = value; } } | ||
758 | public override float APIDDamping { set { m_APIDDamping = value; } } | ||
759 | |||
760 | public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } | ||
761 | public override bool PIDHoverActive { set { m_useHoverPID = value; } } | ||
762 | public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } | ||
763 | public override float PIDHoverTau { set { m_PIDHoverTau = value; } } | ||
764 | |||
765 | internal static bool QuaternionIsFinite(Quaternion q) | ||
766 | { | ||
767 | if (Single.IsNaN(q.X) || Single.IsInfinity(q.X)) | ||
768 | return false; | ||
769 | if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y)) | ||
770 | return false; | ||
771 | if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z)) | ||
772 | return false; | ||
773 | if (Single.IsNaN(q.W) || Single.IsInfinity(q.W)) | ||
774 | return false; | ||
775 | return true; | ||
776 | } | ||
777 | |||
778 | public override Vector3 Acceleration // client updates read data via here | ||
779 | { | ||
780 | get | ||
781 | { | ||
782 | if (_zeroFlag) | ||
783 | { | ||
784 | return Vector3.Zero; | ||
785 | } | ||
786 | return _acceleration; | ||
787 | } | ||
788 | set { _acceleration = value; } | ||
789 | } | ||
790 | |||
791 | |||
792 | public void SetAcceleration(Vector3 accel) // No one calls this, and it would not do anything. | ||
793 | { | ||
794 | _acceleration = accel; | ||
795 | } | ||
796 | |||
797 | public override void AddForce(Vector3 force, bool pushforce) | ||
798 | { | ||
799 | if (force.IsFinite()) | ||
800 | { | ||
801 | lock (m_forcelist) | ||
802 | m_forcelist.Add(force); | ||
803 | |||
804 | m_taintforce = true; | ||
805 | } | ||
806 | else | ||
807 | { | ||
808 | m_log.Warn("[PHYSICS]: Got Invalid linear force vector from Scene in Object"); | ||
809 | } | ||
810 | //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString()); | ||
811 | } | ||
812 | |||
813 | public override void AddAngularForce(Vector3 force, bool pushforce) | ||
814 | { | ||
815 | if (force.IsFinite()) | ||
816 | { | ||
817 | m_angularforcelist.Add(force); | ||
818 | m_taintaddangularforce = true; | ||
819 | } | ||
820 | else | ||
821 | { | ||
822 | m_log.Warn("[PHYSICS]: Got Invalid Angular force vector from Scene in Object"); | ||
823 | } | ||
824 | } | ||
825 | |||
826 | public override Vector3 RotationalVelocity | ||
827 | { | ||
828 | get | ||
829 | { | ||
830 | return m_rotationalVelocity; | ||
831 | } | ||
832 | set | ||
833 | { | ||
834 | if (value.IsFinite()) | ||
835 | { | ||
836 | m_rotationalVelocity = value; | ||
837 | } | ||
838 | else | ||
839 | { | ||
840 | m_log.Warn("[PHYSICS]: Got NaN RotationalVelocity in Object"); | ||
841 | } | ||
842 | } | ||
843 | } | ||
844 | |||
845 | public override void CrossingFailure() | ||
846 | { | ||
847 | if (m_outofBounds) | ||
848 | { | ||
849 | _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f); | ||
850 | _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f); | ||
851 | _position.Z = Util.Clip(_position.Z, -100f, 50000f); | ||
852 | d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); | ||
853 | |||
854 | m_lastposition = _position; | ||
855 | |||
856 | _velocity = Vector3.Zero; | ||
857 | m_lastVelocity = _velocity; | ||
858 | |||
859 | |||
860 | if (m_type != Vehicle.TYPE_NONE) | ||
861 | Halt(); | ||
862 | |||
863 | d.BodySetLinearVel(Body, 0, 0, 0); | ||
864 | base.RequestPhysicsterseUpdate(); | ||
865 | m_outofBounds = false; | ||
866 | } | ||
867 | /* | ||
868 | int tmp = Interlocked.Increment(ref m_crossingfailures); | ||
869 | if (tmp > _parent_scene.geomCrossingFailuresBeforeOutofbounds) | ||
870 | { | ||
871 | base.RaiseOutOfBounds(_position); | ||
872 | return; | ||
873 | } | ||
874 | else if (tmp == _parent_scene.geomCrossingFailuresBeforeOutofbounds) | ||
875 | { | ||
876 | m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName); | ||
877 | } | ||
878 | */ | ||
879 | } | ||
880 | |||
881 | public override float Buoyancy | ||
882 | { | ||
883 | get { return m_buoyancy; } | ||
884 | set { m_buoyancy = value; } | ||
885 | } | ||
886 | |||
887 | public override void link(PhysicsActor obj) | ||
888 | { | ||
889 | m_taintparent = obj; | ||
890 | } | ||
891 | |||
892 | public override void delink() | ||
893 | { | ||
894 | m_taintparent = null; | ||
895 | } | ||
896 | |||
897 | public override void LockAngularMotion(Vector3 axis) | ||
898 | { | ||
899 | // This is actually ROTATION ENABLE, not a lock. | ||
900 | // default is <1,1,1> which is all enabled. | ||
901 | // The lock value is updated inside Move(), no point in using the taint system. | ||
902 | // OS 'm_taintAngularLock' etc change to m_rotateEnable. | ||
903 | if (axis.IsFinite()) | ||
904 | { | ||
905 | axis.X = (axis.X > 0) ? 1f : 0f; | ||
906 | axis.Y = (axis.Y > 0) ? 1f : 0f; | ||
907 | axis.Z = (axis.Z > 0) ? 1f : 0f; | ||
908 | m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); | ||
909 | m_rotateEnableRequest = axis; | ||
910 | m_rotateEnableUpdate = true; | ||
911 | } | ||
912 | else | ||
913 | { | ||
914 | m_log.Warn("[PHYSICS]: Got NaN locking axis from Scene on Object"); | ||
915 | } | ||
916 | } | ||
917 | |||
918 | public void SetGeom(IntPtr geom) | ||
919 | { | ||
920 | if (prim_geom != IntPtr.Zero) | ||
921 | { | ||
922 | // Remove any old entries | ||
923 | //string tPA; | ||
924 | //_parent_scene.geom_name_map.TryGetValue(prim_geom, out tPA); | ||
925 | //Console.WriteLine("**** Remove {0}", tPA); | ||
926 | if (_parent_scene.geom_name_map.ContainsKey(prim_geom)) _parent_scene.geom_name_map.Remove(prim_geom); | ||
927 | if (_parent_scene.actor_name_map.ContainsKey(prim_geom)) _parent_scene.actor_name_map.Remove(prim_geom); | ||
928 | d.GeomDestroy(prim_geom); | ||
929 | } | ||
930 | |||
931 | prim_geom = geom; | ||
932 | //Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName); | ||
933 | if (prim_geom != IntPtr.Zero) | ||
934 | { | ||
935 | _parent_scene.geom_name_map[prim_geom] = this.m_primName; | ||
936 | _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this; | ||
937 | //Console.WriteLine("**** Create {2} Dicts: actor={0} name={1}", _parent_scene.actor_name_map.Count, _parent_scene.geom_name_map.Count, this.m_primName); | ||
938 | if (m_NoColide) | ||
939 | { | ||
940 | d.GeomSetCategoryBits(prim_geom, 0); | ||
941 | if (m_isphysical && !m_isVolumeDetect) | ||
942 | { | ||
943 | d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); | ||
944 | } | ||
945 | else | ||
946 | { | ||
947 | d.GeomSetCollideBits(prim_geom, 0); | ||
948 | d.GeomDisable(prim_geom); | ||
949 | } | ||
950 | } | ||
951 | else | ||
952 | { | ||
953 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | ||
954 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
955 | } | ||
956 | } | ||
957 | |||
958 | if (childPrim) | ||
959 | { | ||
960 | if (_parent != null && _parent is OdePrim) | ||
961 | { | ||
962 | OdePrim parent = (OdePrim)_parent; | ||
963 | //Console.WriteLine("SetGeom calls ChildSetGeom"); | ||
964 | parent.ChildSetGeom(this); | ||
965 | } | ||
966 | } | ||
967 | //m_log.Warn("Setting Geom to: " + prim_geom); | ||
968 | } | ||
969 | |||
970 | public void enableBodySoft() | ||
971 | { | ||
972 | if (!childPrim) | ||
973 | { | ||
974 | if (m_isphysical && Body != IntPtr.Zero) | ||
975 | { | ||
976 | d.BodyEnable(Body); | ||
977 | if (m_type != Vehicle.TYPE_NONE) | ||
978 | Enable(Body, _parent_scene); | ||
979 | } | ||
980 | |||
981 | m_disabled = false; | ||
982 | } | ||
983 | } | ||
984 | |||
985 | public void disableBodySoft() | ||
986 | { | ||
987 | m_disabled = true; | ||
988 | |||
989 | if (m_isphysical && Body != IntPtr.Zero) | ||
990 | { | ||
991 | d.BodyDisable(Body); | ||
992 | Halt(); | ||
993 | } | ||
994 | } | ||
995 | |||
996 | public void enableBody() | ||
997 | { | ||
998 | // Don't enable this body if we're a child prim | ||
999 | // this should be taken care of in the parent function not here | ||
1000 | if (!childPrim) | ||
1001 | { | ||
1002 | // Sets the geom to a body | ||
1003 | Body = d.BodyCreate(_parent_scene.world); | ||
1004 | |||
1005 | setMass(); | ||
1006 | d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); | ||
1007 | d.Quaternion myrot = new d.Quaternion(); | ||
1008 | myrot.X = _orientation.X; | ||
1009 | myrot.Y = _orientation.Y; | ||
1010 | myrot.Z = _orientation.Z; | ||
1011 | myrot.W = _orientation.W; | ||
1012 | d.BodySetQuaternion(Body, ref myrot); | ||
1013 | d.GeomSetBody(prim_geom, Body); | ||
1014 | |||
1015 | m_collisionCategories |= CollisionCategories.Body; | ||
1016 | m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); | ||
1017 | |||
1018 | if (m_NoColide) | ||
1019 | { | ||
1020 | d.GeomSetCategoryBits(prim_geom, 0); | ||
1021 | d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); | ||
1022 | } | ||
1023 | else | ||
1024 | { | ||
1025 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | ||
1026 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
1027 | } | ||
1028 | |||
1029 | d.BodySetAutoDisableFlag(Body, true); | ||
1030 | d.BodySetAutoDisableSteps(Body, body_autodisable_frames); | ||
1031 | |||
1032 | // disconnect from world gravity so we can apply buoyancy | ||
1033 | d.BodySetGravityMode(Body, false); | ||
1034 | |||
1035 | m_interpenetrationcount = 0; | ||
1036 | m_collisionscore = 0; | ||
1037 | m_disabled = false; | ||
1038 | |||
1039 | if (m_type != Vehicle.TYPE_NONE) | ||
1040 | { | ||
1041 | Enable(Body, _parent_scene); | ||
1042 | } | ||
1043 | |||
1044 | _parent_scene.addActivePrim(this); | ||
1045 | } | ||
1046 | } | ||
1047 | |||
1048 | #region Mass Calculation | ||
1049 | |||
1050 | private float CalculateMass() | ||
1051 | { | ||
1052 | float volume = _size.X * _size.Y * _size.Z; // default | ||
1053 | float tmp; | ||
1054 | |||
1055 | float returnMass = 0; | ||
1056 | float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; | ||
1057 | float hollowVolume = hollowAmount * hollowAmount; | ||
1058 | |||
1059 | switch (_pbs.ProfileShape) | ||
1060 | { | ||
1061 | case ProfileShape.Square: | ||
1062 | // default box | ||
1063 | |||
1064 | if (_pbs.PathCurve == (byte)Extrusion.Straight) | ||
1065 | { | ||
1066 | if (hollowAmount > 0.0) | ||
1067 | { | ||
1068 | switch (_pbs.HollowShape) | ||
1069 | { | ||
1070 | case HollowShape.Square: | ||
1071 | case HollowShape.Same: | ||
1072 | break; | ||
1073 | |||
1074 | case HollowShape.Circle: | ||
1075 | |||
1076 | hollowVolume *= 0.78539816339f; | ||
1077 | break; | ||
1078 | |||
1079 | case HollowShape.Triangle: | ||
1080 | |||
1081 | hollowVolume *= (0.5f * .5f); | ||
1082 | break; | ||
1083 | |||
1084 | default: | ||
1085 | hollowVolume = 0; | ||
1086 | break; | ||
1087 | } | ||
1088 | volume *= (1.0f - hollowVolume); | ||
1089 | } | ||
1090 | } | ||
1091 | |||
1092 | else if (_pbs.PathCurve == (byte)Extrusion.Curve1) | ||
1093 | { | ||
1094 | //a tube | ||
1095 | |||
1096 | volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); | ||
1097 | tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY); | ||
1098 | volume -= volume * tmp * tmp; | ||
1099 | |||
1100 | if (hollowAmount > 0.0) | ||
1101 | { | ||
1102 | hollowVolume *= hollowAmount; | ||
1103 | |||
1104 | switch (_pbs.HollowShape) | ||
1105 | { | ||
1106 | case HollowShape.Square: | ||
1107 | case HollowShape.Same: | ||
1108 | break; | ||
1109 | |||
1110 | case HollowShape.Circle: | ||
1111 | hollowVolume *= 0.78539816339f; ; | ||
1112 | break; | ||
1113 | |||
1114 | case HollowShape.Triangle: | ||
1115 | hollowVolume *= 0.5f * 0.5f; | ||
1116 | break; | ||
1117 | default: | ||
1118 | hollowVolume = 0; | ||
1119 | break; | ||
1120 | } | ||
1121 | volume *= (1.0f - hollowVolume); | ||
1122 | } | ||
1123 | } | ||
1124 | |||
1125 | break; | ||
1126 | |||
1127 | case ProfileShape.Circle: | ||
1128 | |||
1129 | if (_pbs.PathCurve == (byte)Extrusion.Straight) | ||
1130 | { | ||
1131 | volume *= 0.78539816339f; // elipse base | ||
1132 | |||
1133 | if (hollowAmount > 0.0) | ||
1134 | { | ||
1135 | switch (_pbs.HollowShape) | ||
1136 | { | ||
1137 | case HollowShape.Same: | ||
1138 | case HollowShape.Circle: | ||
1139 | break; | ||
1140 | |||
1141 | case HollowShape.Square: | ||
1142 | hollowVolume *= 0.5f * 2.5984480504799f; | ||
1143 | break; | ||
1144 | |||
1145 | case HollowShape.Triangle: | ||
1146 | hollowVolume *= .5f * 1.27323954473516f; | ||
1147 | break; | ||
1148 | |||
1149 | default: | ||
1150 | hollowVolume = 0; | ||
1151 | break; | ||
1152 | } | ||
1153 | volume *= (1.0f - hollowVolume); | ||
1154 | } | ||
1155 | } | ||
1156 | |||
1157 | else if (_pbs.PathCurve == (byte)Extrusion.Curve1) | ||
1158 | { | ||
1159 | volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); | ||
1160 | tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); | ||
1161 | volume *= (1.0f - tmp * tmp); | ||
1162 | |||
1163 | if (hollowAmount > 0.0) | ||
1164 | { | ||
1165 | |||
1166 | // calculate the hollow volume by it's shape compared to the prim shape | ||
1167 | hollowVolume *= hollowAmount; | ||
1168 | |||
1169 | switch (_pbs.HollowShape) | ||
1170 | { | ||
1171 | case HollowShape.Same: | ||
1172 | case HollowShape.Circle: | ||
1173 | break; | ||
1174 | |||
1175 | case HollowShape.Square: | ||
1176 | hollowVolume *= 0.5f * 2.5984480504799f; | ||
1177 | break; | ||
1178 | |||
1179 | case HollowShape.Triangle: | ||
1180 | hollowVolume *= .5f * 1.27323954473516f; | ||
1181 | break; | ||
1182 | |||
1183 | default: | ||
1184 | hollowVolume = 0; | ||
1185 | break; | ||
1186 | } | ||
1187 | volume *= (1.0f - hollowVolume); | ||
1188 | } | ||
1189 | } | ||
1190 | break; | ||
1191 | |||
1192 | case ProfileShape.HalfCircle: | ||
1193 | if (_pbs.PathCurve == (byte)Extrusion.Curve1) | ||
1194 | { | ||
1195 | volume *= 0.52359877559829887307710723054658f; | ||
1196 | } | ||
1197 | break; | ||
1198 | |||
1199 | case ProfileShape.EquilateralTriangle: | ||
1200 | |||
1201 | if (_pbs.PathCurve == (byte)Extrusion.Straight) | ||
1202 | { | ||
1203 | volume *= 0.32475953f; | ||
1204 | |||
1205 | if (hollowAmount > 0.0) | ||
1206 | { | ||
1207 | |||
1208 | // calculate the hollow volume by it's shape compared to the prim shape | ||
1209 | switch (_pbs.HollowShape) | ||
1210 | { | ||
1211 | case HollowShape.Same: | ||
1212 | case HollowShape.Triangle: | ||
1213 | hollowVolume *= .25f; | ||
1214 | break; | ||
1215 | |||
1216 | case HollowShape.Square: | ||
1217 | hollowVolume *= 0.499849f * 3.07920140172638f; | ||
1218 | break; | ||
1219 | |||
1220 | case HollowShape.Circle: | ||
1221 | // Hollow shape is a perfect cyllinder in respect to the cube's scale | ||
1222 | // Cyllinder hollow volume calculation | ||
1223 | |||
1224 | hollowVolume *= 0.1963495f * 3.07920140172638f; | ||
1225 | break; | ||
1226 | |||
1227 | default: | ||
1228 | hollowVolume = 0; | ||
1229 | break; | ||
1230 | } | ||
1231 | volume *= (1.0f - hollowVolume); | ||
1232 | } | ||
1233 | } | ||
1234 | else if (_pbs.PathCurve == (byte)Extrusion.Curve1) | ||
1235 | { | ||
1236 | volume *= 0.32475953f; | ||
1237 | volume *= 0.01f * (float)(200 - _pbs.PathScaleX); | ||
1238 | tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); | ||
1239 | volume *= (1.0f - tmp * tmp); | ||
1240 | |||
1241 | if (hollowAmount > 0.0) | ||
1242 | { | ||
1243 | |||
1244 | hollowVolume *= hollowAmount; | ||
1245 | |||
1246 | switch (_pbs.HollowShape) | ||
1247 | { | ||
1248 | case HollowShape.Same: | ||
1249 | case HollowShape.Triangle: | ||
1250 | hollowVolume *= .25f; | ||
1251 | break; | ||
1252 | |||
1253 | case HollowShape.Square: | ||
1254 | hollowVolume *= 0.499849f * 3.07920140172638f; | ||
1255 | break; | ||
1256 | |||
1257 | case HollowShape.Circle: | ||
1258 | |||
1259 | hollowVolume *= 0.1963495f * 3.07920140172638f; | ||
1260 | break; | ||
1261 | |||
1262 | default: | ||
1263 | hollowVolume = 0; | ||
1264 | break; | ||
1265 | } | ||
1266 | volume *= (1.0f - hollowVolume); | ||
1267 | } | ||
1268 | } | ||
1269 | break; | ||
1270 | |||
1271 | default: | ||
1272 | break; | ||
1273 | } | ||
1274 | |||
1275 | |||
1276 | |||
1277 | float taperX1; | ||
1278 | float taperY1; | ||
1279 | float taperX; | ||
1280 | float taperY; | ||
1281 | float pathBegin; | ||
1282 | float pathEnd; | ||
1283 | float profileBegin; | ||
1284 | float profileEnd; | ||
1285 | |||
1286 | if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) | ||
1287 | { | ||
1288 | taperX1 = _pbs.PathScaleX * 0.01f; | ||
1289 | if (taperX1 > 1.0f) | ||
1290 | taperX1 = 2.0f - taperX1; | ||
1291 | taperX = 1.0f - taperX1; | ||
1292 | |||
1293 | taperY1 = _pbs.PathScaleY * 0.01f; | ||
1294 | if (taperY1 > 1.0f) | ||
1295 | taperY1 = 2.0f - taperY1; | ||
1296 | taperY = 1.0f - taperY1; | ||
1297 | } | ||
1298 | else | ||
1299 | { | ||
1300 | taperX = _pbs.PathTaperX * 0.01f; | ||
1301 | if (taperX < 0.0f) | ||
1302 | taperX = -taperX; | ||
1303 | taperX1 = 1.0f - taperX; | ||
1304 | |||
1305 | taperY = _pbs.PathTaperY * 0.01f; | ||
1306 | if (taperY < 0.0f) | ||
1307 | taperY = -taperY; | ||
1308 | taperY1 = 1.0f - taperY; | ||
1309 | |||
1310 | } | ||
1311 | |||
1312 | |||
1313 | volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); | ||
1314 | |||
1315 | pathBegin = (float)_pbs.PathBegin * 2.0e-5f; | ||
1316 | pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; | ||
1317 | volume *= (pathEnd - pathBegin); | ||
1318 | |||
1319 | // this is crude aproximation | ||
1320 | profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; | ||
1321 | profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; | ||
1322 | volume *= (profileEnd - profileBegin); | ||
1323 | |||
1324 | returnMass = m_density * volume; | ||
1325 | |||
1326 | if (returnMass <= 0) | ||
1327 | returnMass = 0.0001f;//ckrinke: Mass must be greater then zero. | ||
1328 | // else if (returnMass > _parent_scene.maximumMassObject) | ||
1329 | // returnMass = _parent_scene.maximumMassObject; | ||
1330 | |||
1331 | |||
1332 | |||
1333 | m_primMass = returnMass; | ||
1334 | if (m_primMass > _parent_scene.maximumMassObject) | ||
1335 | m_primMass = _parent_scene.maximumMassObject; | ||
1336 | |||
1337 | // Recursively calculate mass | ||
1338 | bool HasChildPrim = false; | ||
1339 | lock (childrenPrim) | ||
1340 | { | ||
1341 | if (childrenPrim.Count > 0) | ||
1342 | { | ||
1343 | HasChildPrim = true; | ||
1344 | } | ||
1345 | |||
1346 | } | ||
1347 | if (HasChildPrim) | ||
1348 | { | ||
1349 | OdePrim[] childPrimArr = new OdePrim[0]; | ||
1350 | |||
1351 | lock (childrenPrim) | ||
1352 | childPrimArr = childrenPrim.ToArray(); | ||
1353 | |||
1354 | for (int i = 0; i < childPrimArr.Length; i++) | ||
1355 | { | ||
1356 | if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove) | ||
1357 | returnMass += childPrimArr[i].CalculateMass(); | ||
1358 | // failsafe, this shouldn't happen but with OpenSim, you never know :) | ||
1359 | if (i > 256) | ||
1360 | break; | ||
1361 | } | ||
1362 | } | ||
1363 | if (returnMass > _parent_scene.maximumMassObject) | ||
1364 | returnMass = _parent_scene.maximumMassObject; | ||
1365 | return returnMass; | ||
1366 | }// end CalculateMass | ||
1367 | |||
1368 | #endregion | ||
1369 | |||
1370 | public void setMass() | ||
1371 | { | ||
1372 | if (Body != (IntPtr)0) | ||
1373 | { | ||
1374 | float newmass = CalculateMass(); | ||
1375 | |||
1376 | //m_log.Info("[PHYSICS]: New Mass: " + newmass.ToString()); | ||
1377 | |||
1378 | d.MassSetBoxTotal(out pMass, newmass, _size.X, _size.Y, _size.Z); | ||
1379 | d.BodySetMass(Body, ref pMass); | ||
1380 | } | ||
1381 | } | ||
1382 | |||
1383 | |||
1384 | private void UpdateDataFromGeom() | ||
1385 | { | ||
1386 | if (prim_geom != IntPtr.Zero) | ||
1387 | { | ||
1388 | d.Quaternion qtmp; | ||
1389 | d.GeomCopyQuaternion(prim_geom, out qtmp); | ||
1390 | _orientation.W = qtmp.W; | ||
1391 | _orientation.X = qtmp.X; | ||
1392 | _orientation.Y = qtmp.Y; | ||
1393 | _orientation.Z = qtmp.Z; | ||
1394 | |||
1395 | d.Vector3 lpos = d.GeomGetPosition(prim_geom); | ||
1396 | _position.X = lpos.X; | ||
1397 | _position.Y = lpos.Y; | ||
1398 | _position.Z = lpos.Z; | ||
1399 | } | ||
1400 | } | ||
1401 | |||
1402 | public void disableBody() | ||
1403 | { | ||
1404 | //this kills the body so things like 'mesh' can re-create it. | ||
1405 | lock (this) | ||
1406 | { | ||
1407 | if (!childPrim) | ||
1408 | { | ||
1409 | if (Body != IntPtr.Zero) | ||
1410 | { | ||
1411 | _parent_scene.remActivePrim(this); | ||
1412 | m_collisionCategories &= ~CollisionCategories.Body; | ||
1413 | m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); | ||
1414 | |||
1415 | if (prim_geom != IntPtr.Zero) | ||
1416 | { | ||
1417 | if (m_NoColide) | ||
1418 | { | ||
1419 | d.GeomSetCategoryBits(prim_geom, 0); | ||
1420 | d.GeomSetCollideBits(prim_geom, 0); | ||
1421 | d.GeomDisable(prim_geom); | ||
1422 | } | ||
1423 | else | ||
1424 | { | ||
1425 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | ||
1426 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
1427 | } | ||
1428 | } | ||
1429 | |||
1430 | UpdateDataFromGeom(); | ||
1431 | |||
1432 | lock (childrenPrim) | ||
1433 | { | ||
1434 | if (childrenPrim.Count > 0) | ||
1435 | { | ||
1436 | foreach (OdePrim prm in childrenPrim) | ||
1437 | { | ||
1438 | if (prm.prim_geom != IntPtr.Zero) | ||
1439 | { | ||
1440 | if (prm.m_NoColide) | ||
1441 | { | ||
1442 | d.GeomSetCategoryBits(prm.prim_geom, 0); | ||
1443 | d.GeomSetCollideBits(prm.prim_geom, 0); | ||
1444 | d.GeomDisable(prm.prim_geom); | ||
1445 | |||
1446 | } | ||
1447 | prm.UpdateDataFromGeom(); | ||
1448 | } | ||
1449 | _parent_scene.remActivePrim(prm); | ||
1450 | prm.Body = IntPtr.Zero; | ||
1451 | } | ||
1452 | } | ||
1453 | } | ||
1454 | d.BodyDestroy(Body); | ||
1455 | Body = IntPtr.Zero; | ||
1456 | } | ||
1457 | } | ||
1458 | else | ||
1459 | { | ||
1460 | _parent_scene.remActivePrim(this); | ||
1461 | |||
1462 | m_collisionCategories &= ~CollisionCategories.Body; | ||
1463 | m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); | ||
1464 | |||
1465 | if (prim_geom != IntPtr.Zero) | ||
1466 | { | ||
1467 | if (m_NoColide) | ||
1468 | { | ||
1469 | d.GeomSetCategoryBits(prim_geom, 0); | ||
1470 | d.GeomSetCollideBits(prim_geom, 0); | ||
1471 | d.GeomDisable(prim_geom); | ||
1472 | } | ||
1473 | else | ||
1474 | { | ||
1475 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | ||
1476 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
1477 | } | ||
1478 | } | ||
1479 | |||
1480 | Body = IntPtr.Zero; | ||
1481 | } | ||
1482 | } | ||
1483 | m_disabled = true; | ||
1484 | m_collisionscore = 0; | ||
1485 | } | ||
1486 | |||
1487 | // private static Dictionary<IMesh, IntPtr> m_MeshToTriMeshMap = new Dictionary<IMesh, IntPtr>(); | ||
1488 | |||
1489 | public bool setMesh(OdeScene parent_scene, IMesh mesh) | ||
1490 | { | ||
1491 | //Kill Body so that mesh can re-make the geom | ||
1492 | if (IsPhysical && Body != IntPtr.Zero) | ||
1493 | { | ||
1494 | if (childPrim) | ||
1495 | { | ||
1496 | if (_parent != null) | ||
1497 | { | ||
1498 | OdePrim parent = (OdePrim)_parent; | ||
1499 | parent.ChildDelink(this); | ||
1500 | } | ||
1501 | } | ||
1502 | else | ||
1503 | { | ||
1504 | disableBody(); | ||
1505 | } | ||
1506 | } | ||
1507 | |||
1508 | IntPtr vertices, indices; | ||
1509 | int vertexCount, indexCount; | ||
1510 | int vertexStride, triStride; | ||
1511 | mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap | ||
1512 | mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage | ||
1513 | |||
1514 | // warning this destroys the mesh for eventual future use. Only pinned float arrays stay valid | ||
1515 | mesh.releaseSourceMeshData(); // free up the original mesh data to save memory | ||
1516 | |||
1517 | if (vertexCount == 0 || indexCount == 0) | ||
1518 | { | ||
1519 | m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. mesh UUID {4}", Name, _position.X, _position.Y, _position.Z, _pbs.SculptTexture.ToString()); | ||
1520 | return false; | ||
1521 | } | ||
1522 | |||
1523 | IntPtr geo = IntPtr.Zero; | ||
1524 | try | ||
1525 | { | ||
1526 | _triMeshData = d.GeomTriMeshDataCreate(); | ||
1527 | d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); | ||
1528 | d.GeomTriMeshDataPreprocess(_triMeshData); | ||
1529 | |||
1530 | _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
1531 | |||
1532 | geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null); | ||
1533 | } | ||
1534 | catch (Exception e) | ||
1535 | { | ||
1536 | m_log.ErrorFormat("[PHYSICS]: Create trimesh failed on prim {0} : {1}",Name,e.Message); | ||
1537 | |||
1538 | if (_triMeshData != IntPtr.Zero) | ||
1539 | { | ||
1540 | d.GeomTriMeshDataDestroy(_triMeshData); | ||
1541 | _triMeshData = IntPtr.Zero; | ||
1542 | } | ||
1543 | return false; | ||
1544 | } | ||
1545 | |||
1546 | SetGeom(geo); | ||
1547 | |||
1548 | return true; | ||
1549 | } | ||
1550 | |||
1551 | public void ProcessTaints(float timestep) //============================================================================= | ||
1552 | { | ||
1553 | if (m_taintadd) | ||
1554 | { | ||
1555 | changeadd(timestep); | ||
1556 | } | ||
1557 | |||
1558 | if (m_taintremove) | ||
1559 | return; | ||
1560 | |||
1561 | if (prim_geom != IntPtr.Zero) | ||
1562 | { | ||
1563 | if (!_position.ApproxEquals(m_taintposition, 0f)) | ||
1564 | { | ||
1565 | changemove(timestep); | ||
1566 | } | ||
1567 | if (m_taintrot != _orientation) | ||
1568 | { | ||
1569 | if (childPrim && IsPhysical) // For physical child prim... | ||
1570 | { | ||
1571 | rotate(timestep); | ||
1572 | // KF: ODE will also rotate the parent prim! | ||
1573 | // so rotate the root back to where it was | ||
1574 | OdePrim parent = (OdePrim)_parent; | ||
1575 | parent.rotate(timestep); | ||
1576 | } | ||
1577 | else | ||
1578 | { | ||
1579 | //Just rotate the prim | ||
1580 | rotate(timestep); | ||
1581 | } | ||
1582 | } | ||
1583 | // | ||
1584 | if (m_taintphantom != m_isphantom ) | ||
1585 | { | ||
1586 | changePhantomStatus(); | ||
1587 | }// | ||
1588 | |||
1589 | if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent)) | ||
1590 | { | ||
1591 | changePhysicsStatus(timestep); | ||
1592 | }// | ||
1593 | |||
1594 | |||
1595 | if (!_size.ApproxEquals(m_taintsize, 0f)) | ||
1596 | changesize(timestep); | ||
1597 | // | ||
1598 | |||
1599 | if(m_taintshapetype != m_shapetype) | ||
1600 | { | ||
1601 | m_shapetype = m_taintshapetype; | ||
1602 | changeshape(timestep); | ||
1603 | } | ||
1604 | |||
1605 | if (m_taintshape) | ||
1606 | changeshape(timestep); | ||
1607 | // | ||
1608 | |||
1609 | if (m_taintforce) | ||
1610 | changeAddForce(timestep); | ||
1611 | |||
1612 | if (m_taintaddangularforce) | ||
1613 | changeAddAngularForce(timestep); | ||
1614 | |||
1615 | if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f)) | ||
1616 | changeSetTorque(timestep); | ||
1617 | |||
1618 | if (m_taintdisable) | ||
1619 | changedisable(timestep); | ||
1620 | |||
1621 | if (m_taintselected != m_isSelected) | ||
1622 | changeSelectedStatus(); | ||
1623 | |||
1624 | if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f)) | ||
1625 | changevelocity(timestep); | ||
1626 | |||
1627 | if (m_taintparent != _parent) | ||
1628 | changelink(timestep); | ||
1629 | |||
1630 | if (m_taintCollidesWater != m_collidesWater) | ||
1631 | changefloatonwater(timestep); | ||
1632 | |||
1633 | if (m_taintvehicledata != null) | ||
1634 | DoSetVehicle(); | ||
1635 | |||
1636 | /* obsolete | ||
1637 | if (!m_angularLock.ApproxEquals(m_taintAngularLock,0f)) | ||
1638 | changeAngularLock(timestep); | ||
1639 | */ | ||
1640 | } | ||
1641 | |||
1642 | else | ||
1643 | { | ||
1644 | m_log.Error("[PHYSICS]: prim {0} at <{1},{2},{3}> as invalid geom"); | ||
1645 | |||
1646 | // not sure this will not flame... | ||
1647 | m_taintremove = true; | ||
1648 | _parent_scene.AddPhysicsActorTaint(this); | ||
1649 | } | ||
1650 | |||
1651 | } | ||
1652 | |||
1653 | private void changelink(float timestep) | ||
1654 | { | ||
1655 | // If the newly set parent is not null | ||
1656 | // create link | ||
1657 | if (_parent == null && m_taintparent != null) | ||
1658 | { | ||
1659 | if (m_taintparent.PhysicsActorType == (int)ActorTypes.Prim) | ||
1660 | { | ||
1661 | OdePrim obj = (OdePrim)m_taintparent; | ||
1662 | obj.ParentPrim(this); | ||
1663 | } | ||
1664 | } | ||
1665 | // If the newly set parent is null | ||
1666 | // destroy link | ||
1667 | else if (_parent != null && m_taintparent == null) | ||
1668 | { | ||
1669 | if (_parent is OdePrim) | ||
1670 | { | ||
1671 | OdePrim obj = (OdePrim)_parent; | ||
1672 | obj.ChildDelink(this); | ||
1673 | childPrim = false; | ||
1674 | } | ||
1675 | } | ||
1676 | |||
1677 | _parent = m_taintparent; | ||
1678 | m_taintPhysics = m_isphysical; | ||
1679 | } | ||
1680 | |||
1681 | // I'm the parent | ||
1682 | // prim is the child | ||
1683 | public void ParentPrim(OdePrim prim) | ||
1684 | { | ||
1685 | if (this.m_localID != prim.m_localID) | ||
1686 | { | ||
1687 | if (Body == IntPtr.Zero) | ||
1688 | { | ||
1689 | Body = d.BodyCreate(_parent_scene.world); | ||
1690 | // disconnect from world gravity so we can apply buoyancy | ||
1691 | d.BodySetGravityMode(Body, false); | ||
1692 | |||
1693 | setMass(); | ||
1694 | } | ||
1695 | if (Body != IntPtr.Zero) | ||
1696 | { | ||
1697 | lock (childrenPrim) | ||
1698 | { | ||
1699 | if (!childrenPrim.Contains(prim)) | ||
1700 | { | ||
1701 | childrenPrim.Add(prim); | ||
1702 | |||
1703 | foreach (OdePrim prm in childrenPrim) | ||
1704 | { | ||
1705 | d.Mass m2; | ||
1706 | d.MassSetZero(out m2); | ||
1707 | d.MassSetBoxTotal(out m2, prim.CalculateMass(), prm._size.X, prm._size.Y, prm._size.Z); | ||
1708 | |||
1709 | |||
1710 | d.Quaternion quat = new d.Quaternion(); | ||
1711 | quat.W = prm._orientation.W; | ||
1712 | quat.X = prm._orientation.X; | ||
1713 | quat.Y = prm._orientation.Y; | ||
1714 | quat.Z = prm._orientation.Z; | ||
1715 | |||
1716 | d.Matrix3 mat = new d.Matrix3(); | ||
1717 | d.RfromQ(out mat, ref quat); | ||
1718 | d.MassRotate(ref m2, ref mat); | ||
1719 | d.MassTranslate(ref m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z); | ||
1720 | d.MassAdd(ref pMass, ref m2); | ||
1721 | } | ||
1722 | foreach (OdePrim prm in childrenPrim) | ||
1723 | { | ||
1724 | if (m_isphantom && !prm.m_isVolumeDetect) | ||
1725 | { | ||
1726 | prm.m_collisionCategories = 0; | ||
1727 | prm.m_collisionFlags = CollisionCategories.Land; | ||
1728 | } | ||
1729 | else | ||
1730 | { | ||
1731 | prm.m_collisionCategories |= CollisionCategories.Body; | ||
1732 | prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); | ||
1733 | } | ||
1734 | if (prm.prim_geom == IntPtr.Zero) | ||
1735 | { | ||
1736 | m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements. No geom yet"); | ||
1737 | continue; | ||
1738 | } | ||
1739 | |||
1740 | if (prm.m_NoColide) | ||
1741 | { | ||
1742 | d.GeomSetCategoryBits(prm.prim_geom, 0); | ||
1743 | d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); | ||
1744 | } | ||
1745 | else | ||
1746 | { | ||
1747 | d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); | ||
1748 | d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); | ||
1749 | } | ||
1750 | |||
1751 | d.Quaternion quat = new d.Quaternion(); | ||
1752 | quat.W = prm._orientation.W; | ||
1753 | quat.X = prm._orientation.X; | ||
1754 | quat.Y = prm._orientation.Y; | ||
1755 | quat.Z = prm._orientation.Z; | ||
1756 | |||
1757 | d.Matrix3 mat = new d.Matrix3(); | ||
1758 | d.RfromQ(out mat, ref quat); | ||
1759 | if (Body != IntPtr.Zero) | ||
1760 | { | ||
1761 | d.GeomSetBody(prm.prim_geom, Body); | ||
1762 | prm.childPrim = true; | ||
1763 | d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X, prm.Position.Y, prm.Position.Z); | ||
1764 | //d.GeomSetOffsetPosition(prim.prim_geom, | ||
1765 | // (Position.X - prm.Position.X) - pMass.c.X, | ||
1766 | // (Position.Y - prm.Position.Y) - pMass.c.Y, | ||
1767 | // (Position.Z - prm.Position.Z) - pMass.c.Z); | ||
1768 | d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); | ||
1769 | //d.GeomSetOffsetRotation(prm.prim_geom, ref mat); | ||
1770 | d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z); | ||
1771 | d.BodySetMass(Body, ref pMass); | ||
1772 | } | ||
1773 | else | ||
1774 | { | ||
1775 | m_log.Debug("[PHYSICS]:I ain't got no boooooooooddy, no body"); | ||
1776 | } | ||
1777 | |||
1778 | prm.m_interpenetrationcount = 0; | ||
1779 | prm.m_collisionscore = 0; | ||
1780 | prm.m_disabled = false; | ||
1781 | |||
1782 | prm.Body = Body; | ||
1783 | |||
1784 | _parent_scene.addActivePrim(prm); | ||
1785 | } | ||
1786 | |||
1787 | if (m_isphantom && !m_isVolumeDetect) | ||
1788 | { | ||
1789 | m_collisionCategories = 0; | ||
1790 | m_collisionFlags = CollisionCategories.Land; | ||
1791 | } | ||
1792 | else | ||
1793 | { | ||
1794 | m_collisionCategories |= CollisionCategories.Body; | ||
1795 | m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); | ||
1796 | } | ||
1797 | |||
1798 | if (m_NoColide) | ||
1799 | { | ||
1800 | d.GeomSetCategoryBits(prim_geom, 0); | ||
1801 | d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); | ||
1802 | } | ||
1803 | else | ||
1804 | { | ||
1805 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | ||
1806 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
1807 | } | ||
1808 | |||
1809 | d.Quaternion quat2 = new d.Quaternion(); | ||
1810 | quat2.W = _orientation.W; | ||
1811 | quat2.X = _orientation.X; | ||
1812 | quat2.Y = _orientation.Y; | ||
1813 | quat2.Z = _orientation.Z; | ||
1814 | |||
1815 | d.Matrix3 mat2 = new d.Matrix3(); | ||
1816 | d.RfromQ(out mat2, ref quat2); | ||
1817 | d.GeomSetBody(prim_geom, Body); | ||
1818 | d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z); | ||
1819 | //d.GeomSetOffsetPosition(prim.prim_geom, | ||
1820 | // (Position.X - prm.Position.X) - pMass.c.X, | ||
1821 | // (Position.Y - prm.Position.Y) - pMass.c.Y, | ||
1822 | // (Position.Z - prm.Position.Z) - pMass.c.Z); | ||
1823 | //d.GeomSetOffsetRotation(prim_geom, ref mat2); | ||
1824 | d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z); | ||
1825 | d.BodySetMass(Body, ref pMass); | ||
1826 | |||
1827 | d.BodySetAutoDisableFlag(Body, true); | ||
1828 | d.BodySetAutoDisableSteps(Body, body_autodisable_frames); | ||
1829 | |||
1830 | m_interpenetrationcount = 0; | ||
1831 | m_collisionscore = 0; | ||
1832 | m_disabled = false; | ||
1833 | |||
1834 | d.BodySetPosition(Body, Position.X, Position.Y, Position.Z); | ||
1835 | if (m_type != Vehicle.TYPE_NONE) Enable(Body, _parent_scene); | ||
1836 | |||
1837 | _parent_scene.addActivePrim(this); | ||
1838 | } | ||
1839 | } | ||
1840 | } | ||
1841 | } | ||
1842 | } | ||
1843 | |||
1844 | private void ChildSetGeom(OdePrim odePrim) | ||
1845 | { | ||
1846 | lock (childrenPrim) | ||
1847 | { | ||
1848 | foreach (OdePrim prm in childrenPrim) | ||
1849 | { | ||
1850 | prm.disableBody(); | ||
1851 | } | ||
1852 | } | ||
1853 | disableBody(); | ||
1854 | |||
1855 | if (Body != IntPtr.Zero) | ||
1856 | { | ||
1857 | _parent_scene.remActivePrim(this); | ||
1858 | } | ||
1859 | |||
1860 | lock (childrenPrim) | ||
1861 | { | ||
1862 | foreach (OdePrim prm in childrenPrim) | ||
1863 | { | ||
1864 | ParentPrim(prm); | ||
1865 | } | ||
1866 | } | ||
1867 | } | ||
1868 | |||
1869 | private void ChildDelink(OdePrim odePrim) | ||
1870 | { | ||
1871 | // Okay, we have a delinked child.. need to rebuild the body. | ||
1872 | lock (childrenPrim) | ||
1873 | { | ||
1874 | foreach (OdePrim prm in childrenPrim) | ||
1875 | { | ||
1876 | prm.childPrim = true; | ||
1877 | prm.disableBody(); | ||
1878 | } | ||
1879 | } | ||
1880 | disableBody(); | ||
1881 | |||
1882 | lock (childrenPrim) | ||
1883 | { | ||
1884 | childrenPrim.Remove(odePrim); | ||
1885 | } | ||
1886 | |||
1887 | if (Body != IntPtr.Zero) | ||
1888 | { | ||
1889 | _parent_scene.remActivePrim(this); | ||
1890 | } | ||
1891 | |||
1892 | lock (childrenPrim) | ||
1893 | { | ||
1894 | foreach (OdePrim prm in childrenPrim) | ||
1895 | { | ||
1896 | ParentPrim(prm); | ||
1897 | } | ||
1898 | } | ||
1899 | } | ||
1900 | |||
1901 | private void changePhantomStatus() | ||
1902 | { | ||
1903 | m_taintphantom = m_isphantom; | ||
1904 | changeSelectedStatus(); | ||
1905 | } | ||
1906 | |||
1907 | /* not in use | ||
1908 | private void SetCollider() | ||
1909 | { | ||
1910 | SetCollider(m_isSelected, m_isphysical, m_isphantom, m_isSelected); | ||
1911 | } | ||
1912 | |||
1913 | private void SetCollider(bool sel, bool phys, bool phan, bool vdtc) | ||
1914 | { | ||
1915 | if (sel) | ||
1916 | { | ||
1917 | m_collisionCategories = CollisionCategories.Selected; | ||
1918 | m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); | ||
1919 | } | ||
1920 | else | ||
1921 | { | ||
1922 | if (phan && !vdtc) | ||
1923 | { | ||
1924 | m_collisionCategories = 0; | ||
1925 | if (phys) | ||
1926 | m_collisionFlags = CollisionCategories.Land; | ||
1927 | else | ||
1928 | m_collisionFlags = 0; // this case should not happen non physical phantoms should not have physics | ||
1929 | } | ||
1930 | else | ||
1931 | { | ||
1932 | m_collisionCategories = CollisionCategories.Geom; | ||
1933 | if (phys) | ||
1934 | m_collisionCategories |= CollisionCategories.Body; | ||
1935 | |||
1936 | m_collisionFlags = m_default_collisionFlags; | ||
1937 | |||
1938 | if (m_collidesLand) | ||
1939 | m_collisionFlags |= CollisionCategories.Land; | ||
1940 | if (m_collidesWater) | ||
1941 | m_collisionFlags |= CollisionCategories.Water; | ||
1942 | } | ||
1943 | } | ||
1944 | |||
1945 | if (prim_geom != IntPtr.Zero) | ||
1946 | { | ||
1947 | if (m_NoColide) | ||
1948 | { | ||
1949 | d.GeomSetCategoryBits(prim_geom, 0); | ||
1950 | if (phys) | ||
1951 | d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); | ||
1952 | else | ||
1953 | { | ||
1954 | d.GeomSetCollideBits(prim_geom, 0); | ||
1955 | d.GeomDisable(prim_geom); | ||
1956 | } | ||
1957 | } | ||
1958 | else | ||
1959 | { | ||
1960 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | ||
1961 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
1962 | } | ||
1963 | } | ||
1964 | } | ||
1965 | */ | ||
1966 | |||
1967 | private void changeSelectedStatus() | ||
1968 | { | ||
1969 | if (m_taintselected) | ||
1970 | { | ||
1971 | m_collisionCategories = CollisionCategories.Selected; | ||
1972 | m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); | ||
1973 | |||
1974 | // We do the body disable soft twice because 'in theory' a collision could have happened | ||
1975 | // in between the disabling and the collision properties setting | ||
1976 | // which would wake the physical body up from a soft disabling and potentially cause it to fall | ||
1977 | // through the ground. | ||
1978 | |||
1979 | // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select | ||
1980 | // just one part of the assembly, the rest of the assembly is non-selected and still simulating, | ||
1981 | // so that causes the selected part to wake up and continue moving. | ||
1982 | |||
1983 | // even if you select all parts of a jointed assembly, it is not guaranteed that the entire | ||
1984 | // assembly will stop simulating during the selection, because of the lack of atomicity | ||
1985 | // of select operations (their processing could be interrupted by a thread switch, causing | ||
1986 | // simulation to continue before all of the selected object notifications trickle down to | ||
1987 | // the physics engine). | ||
1988 | |||
1989 | // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are | ||
1990 | // selected and disabled. then, due to a thread switch, the selection processing is | ||
1991 | // interrupted and the physics engine continues to simulate, so the last 50 items, whose | ||
1992 | // selection was not yet processed, continues to simulate. this wakes up ALL of the | ||
1993 | // first 50 again. then the last 50 are disabled. then the first 50, which were just woken | ||
1994 | // up, start simulating again, which in turn wakes up the last 50. | ||
1995 | |||
1996 | if (m_isphysical) | ||
1997 | { | ||
1998 | disableBodySoft(); | ||
1999 | } | ||
2000 | |||
2001 | if (prim_geom != IntPtr.Zero) | ||
2002 | { | ||
2003 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | ||
2004 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
2005 | if (m_NoColide) | ||
2006 | d.GeomDisable(prim_geom); | ||
2007 | } | ||
2008 | |||
2009 | if (m_isphysical) | ||
2010 | { | ||
2011 | disableBodySoft(); | ||
2012 | } | ||
2013 | if (Body != IntPtr.Zero) | ||
2014 | { | ||
2015 | d.BodySetLinearVel(Body, 0f, 0f, 0f); | ||
2016 | d.BodySetForce(Body, 0f, 0f, 0f); | ||
2017 | d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f); | ||
2018 | d.BodySetTorque(Body, 0.0f, 0.0f, 0.0f); | ||
2019 | } | ||
2020 | } | ||
2021 | else | ||
2022 | { | ||
2023 | if (m_isphantom && !m_isVolumeDetect) | ||
2024 | { | ||
2025 | m_collisionCategories = 0; | ||
2026 | if (m_isphysical) | ||
2027 | m_collisionFlags = CollisionCategories.Land; | ||
2028 | else | ||
2029 | m_collisionFlags = 0; // this case should not happen non physical phantoms should not have physics | ||
2030 | } | ||
2031 | else | ||
2032 | { | ||
2033 | m_collisionCategories = CollisionCategories.Geom; | ||
2034 | if (m_isphysical) | ||
2035 | m_collisionCategories |= CollisionCategories.Body; | ||
2036 | |||
2037 | m_collisionFlags = m_default_collisionFlags; | ||
2038 | |||
2039 | if (m_collidesLand) | ||
2040 | m_collisionFlags |= CollisionCategories.Land; | ||
2041 | if (m_collidesWater) | ||
2042 | m_collisionFlags |= CollisionCategories.Water; | ||
2043 | } | ||
2044 | |||
2045 | if (prim_geom != IntPtr.Zero) | ||
2046 | { | ||
2047 | if (m_NoColide) | ||
2048 | { | ||
2049 | d.GeomSetCategoryBits(prim_geom, 0); | ||
2050 | if (m_isphysical) | ||
2051 | d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); | ||
2052 | else | ||
2053 | { | ||
2054 | d.GeomSetCollideBits(prim_geom, 0); | ||
2055 | d.GeomDisable(prim_geom); | ||
2056 | } | ||
2057 | } | ||
2058 | else | ||
2059 | { | ||
2060 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | ||
2061 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
2062 | } | ||
2063 | } | ||
2064 | if (Body != IntPtr.Zero) | ||
2065 | { | ||
2066 | d.BodySetLinearVel(Body, 0f, 0f, 0f); | ||
2067 | d.BodySetForce(Body, 0f, 0f, 0f); | ||
2068 | d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f); | ||
2069 | d.BodySetTorque(Body, 0.0f, 0.0f, 0.0f); | ||
2070 | } | ||
2071 | |||
2072 | if (m_isphysical) | ||
2073 | { | ||
2074 | if (Body != IntPtr.Zero) | ||
2075 | { | ||
2076 | enableBodySoft(); | ||
2077 | } | ||
2078 | } | ||
2079 | } | ||
2080 | |||
2081 | resetCollisionAccounting(); | ||
2082 | m_isSelected = m_taintselected; | ||
2083 | }//end changeSelectedStatus | ||
2084 | |||
2085 | public void ResetTaints() | ||
2086 | { | ||
2087 | m_taintposition = _position; | ||
2088 | m_taintrot = _orientation; | ||
2089 | m_taintPhysics = m_isphysical; | ||
2090 | m_taintselected = m_isSelected; | ||
2091 | m_taintsize = _size; | ||
2092 | m_taintshape = false; | ||
2093 | m_taintforce = false; | ||
2094 | m_taintdisable = false; | ||
2095 | m_taintVelocity = Vector3.Zero; | ||
2096 | } | ||
2097 | |||
2098 | public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh) | ||
2099 | { | ||
2100 | bool gottrimesh = false; | ||
2101 | |||
2102 | m_NoColide = false; // assume all will go well | ||
2103 | |||
2104 | if (_triMeshData != IntPtr.Zero) | ||
2105 | { | ||
2106 | d.GeomTriMeshDataDestroy(_triMeshData); | ||
2107 | _triMeshData = IntPtr.Zero; | ||
2108 | } | ||
2109 | |||
2110 | if (_mesh != null) | ||
2111 | { | ||
2112 | gottrimesh = setMesh(_parent_scene, _mesh); | ||
2113 | if (!gottrimesh) | ||
2114 | { | ||
2115 | // getting a mesh failed, | ||
2116 | // lets go on having a basic box or sphere, with prim size but not coliding | ||
2117 | // physical colides with land, non with nothing | ||
2118 | |||
2119 | m_NoColide = true; | ||
2120 | } | ||
2121 | } | ||
2122 | |||
2123 | if (!gottrimesh) | ||
2124 | { // we will have a basic box or sphere | ||
2125 | IntPtr geo = IntPtr.Zero; | ||
2126 | |||
2127 | if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1 | ||
2128 | && _size.X == _size.Y && _size.X == _size.Z) | ||
2129 | { | ||
2130 | // its a sphere | ||
2131 | _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
2132 | try | ||
2133 | { | ||
2134 | geo = d.CreateSphere(m_targetSpace, _size.X * 0.5f); | ||
2135 | } | ||
2136 | catch (Exception e) | ||
2137 | { | ||
2138 | m_log.WarnFormat("[PHYSICS]: Unable to create basic sphere for object {0}", e.Message); | ||
2139 | geo = IntPtr.Zero; | ||
2140 | ode.dunlock(_parent_scene.world); | ||
2141 | } | ||
2142 | } | ||
2143 | else // make it a box | ||
2144 | { | ||
2145 | _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
2146 | try | ||
2147 | { | ||
2148 | geo = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z); | ||
2149 | } | ||
2150 | catch (Exception e) | ||
2151 | { | ||
2152 | m_log.WarnFormat("[PHYSICS]: Unable to create basic sphere for object {0}", e.Message); | ||
2153 | geo = IntPtr.Zero; | ||
2154 | ode.dunlock(_parent_scene.world); | ||
2155 | } | ||
2156 | } | ||
2157 | |||
2158 | if (geo == IntPtr.Zero) // if this happens it must be fixed | ||
2159 | { | ||
2160 | // if it does lets stop what we can | ||
2161 | // not sure this will not flame... | ||
2162 | |||
2163 | m_taintremove = true; | ||
2164 | _parent_scene.AddPhysicsActorTaint(this); | ||
2165 | return; | ||
2166 | } | ||
2167 | |||
2168 | SetGeom(geo); // this processes the m_NoColide | ||
2169 | } | ||
2170 | } | ||
2171 | |||
2172 | public void changeadd(float timestep) | ||
2173 | { | ||
2174 | int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position); | ||
2175 | IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position); | ||
2176 | |||
2177 | if (targetspace == IntPtr.Zero) | ||
2178 | targetspace = _parent_scene.createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]); | ||
2179 | |||
2180 | m_targetSpace = targetspace; | ||
2181 | |||
2182 | if (_mesh == null) // && m_meshfailed == false) | ||
2183 | { | ||
2184 | if (_parent_scene.needsMeshing(_pbs)) | ||
2185 | { | ||
2186 | bool convex; | ||
2187 | if (m_shapetype == 2) | ||
2188 | convex = true; | ||
2189 | else | ||
2190 | convex = false; | ||
2191 | try | ||
2192 | { | ||
2193 | _mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, (int)LevelOfDetail.High, true,false,convex,false); | ||
2194 | } | ||
2195 | catch | ||
2196 | { | ||
2197 | //Don't continuously try to mesh prims when meshing has failed | ||
2198 | m_meshfailed = true; | ||
2199 | _mesh = null; | ||
2200 | m_log.WarnFormat("[PHYSICS]: changeAdd CreateMesh fail on prim {0} at <{1},{2},{3}>", Name, _position.X, _position.Y, _position.Z); | ||
2201 | } | ||
2202 | } | ||
2203 | } | ||
2204 | |||
2205 | lock (_parent_scene.OdeLock) | ||
2206 | { | ||
2207 | CreateGeom(m_targetSpace, _mesh); | ||
2208 | |||
2209 | if (prim_geom != IntPtr.Zero) | ||
2210 | { | ||
2211 | d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); | ||
2212 | d.Quaternion myrot = new d.Quaternion(); | ||
2213 | myrot.X = _orientation.X; | ||
2214 | myrot.Y = _orientation.Y; | ||
2215 | myrot.Z = _orientation.Z; | ||
2216 | myrot.W = _orientation.W; | ||
2217 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
2218 | } | ||
2219 | |||
2220 | if (m_isphysical && Body == IntPtr.Zero) | ||
2221 | { | ||
2222 | enableBody(); | ||
2223 | } | ||
2224 | } | ||
2225 | |||
2226 | changeSelectedStatus(); | ||
2227 | |||
2228 | m_taintadd = false; | ||
2229 | } | ||
2230 | |||
2231 | public void changemove(float timestep) | ||
2232 | { | ||
2233 | if (m_isphysical) | ||
2234 | { | ||
2235 | // if (!m_disabled && !m_taintremove && !childPrim) After one edit m_disabled is sometimes set, disabling further edits! | ||
2236 | if (!m_taintremove && !childPrim) | ||
2237 | { | ||
2238 | if (Body == IntPtr.Zero) | ||
2239 | enableBody(); | ||
2240 | //Prim auto disable after 20 frames, | ||
2241 | //if you move it, re-enable the prim manually. | ||
2242 | if (_parent != null) | ||
2243 | { | ||
2244 | if (m_linkJoint != IntPtr.Zero) | ||
2245 | { | ||
2246 | d.JointDestroy(m_linkJoint); | ||
2247 | m_linkJoint = IntPtr.Zero; | ||
2248 | } | ||
2249 | } | ||
2250 | if (Body != IntPtr.Zero) | ||
2251 | { | ||
2252 | d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); | ||
2253 | |||
2254 | if (_parent != null) | ||
2255 | { | ||
2256 | OdePrim odParent = (OdePrim)_parent; | ||
2257 | if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body) | ||
2258 | { | ||
2259 | // KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used?? | ||
2260 | Console.WriteLine("ODEPrim JointCreateFixed !!!"); | ||
2261 | m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); | ||
2262 | d.JointAttach(m_linkJoint, Body, odParent.Body); | ||
2263 | d.JointSetFixed(m_linkJoint); | ||
2264 | } | ||
2265 | } | ||
2266 | d.BodyEnable(Body); | ||
2267 | if (m_type != Vehicle.TYPE_NONE) | ||
2268 | { | ||
2269 | Enable(Body, _parent_scene); | ||
2270 | } | ||
2271 | } | ||
2272 | else | ||
2273 | { | ||
2274 | m_log.Warn("[PHYSICS]: Body Still null after enableBody(). This is a crash scenario."); | ||
2275 | } | ||
2276 | } | ||
2277 | //else | ||
2278 | // { | ||
2279 | //m_log.Debug("[BUG]: race!"); | ||
2280 | //} | ||
2281 | } | ||
2282 | else | ||
2283 | { | ||
2284 | // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); | ||
2285 | // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); | ||
2286 | _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
2287 | |||
2288 | IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, _position, m_targetSpace); | ||
2289 | m_targetSpace = tempspace; | ||
2290 | |||
2291 | _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
2292 | if (prim_geom != IntPtr.Zero) | ||
2293 | { | ||
2294 | d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); | ||
2295 | |||
2296 | _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
2297 | d.SpaceAdd(m_targetSpace, prim_geom); | ||
2298 | } | ||
2299 | } | ||
2300 | |||
2301 | changeSelectedStatus(); | ||
2302 | |||
2303 | resetCollisionAccounting(); | ||
2304 | m_taintposition = _position; | ||
2305 | } | ||
2306 | |||
2307 | public void rotate(float timestep) | ||
2308 | { | ||
2309 | d.Quaternion myrot = new d.Quaternion(); | ||
2310 | myrot.X = _orientation.X; | ||
2311 | myrot.Y = _orientation.Y; | ||
2312 | myrot.Z = _orientation.Z; | ||
2313 | myrot.W = _orientation.W; | ||
2314 | if (Body != IntPtr.Zero) | ||
2315 | { | ||
2316 | // KF: If this is a root prim do BodySet | ||
2317 | d.BodySetQuaternion(Body, ref myrot); | ||
2318 | } | ||
2319 | else | ||
2320 | { | ||
2321 | // daughter prim, do Geom set | ||
2322 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
2323 | } | ||
2324 | |||
2325 | resetCollisionAccounting(); | ||
2326 | m_taintrot = _orientation; | ||
2327 | } | ||
2328 | |||
2329 | private void resetCollisionAccounting() | ||
2330 | { | ||
2331 | m_collisionscore = 0; | ||
2332 | m_interpenetrationcount = 0; | ||
2333 | m_disabled = false; | ||
2334 | } | ||
2335 | |||
2336 | public void changedisable(float timestep) | ||
2337 | { | ||
2338 | m_disabled = true; | ||
2339 | if (Body != IntPtr.Zero) | ||
2340 | { | ||
2341 | d.BodyDisable(Body); | ||
2342 | Body = IntPtr.Zero; | ||
2343 | } | ||
2344 | |||
2345 | m_taintdisable = false; | ||
2346 | } | ||
2347 | |||
2348 | public void changePhysicsStatus(float timestep) | ||
2349 | { | ||
2350 | if (m_isphysical == true) | ||
2351 | { | ||
2352 | if (Body == IntPtr.Zero) | ||
2353 | { | ||
2354 | if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) | ||
2355 | { | ||
2356 | changeshape(2f); | ||
2357 | } | ||
2358 | else | ||
2359 | { | ||
2360 | enableBody(); | ||
2361 | } | ||
2362 | } | ||
2363 | } | ||
2364 | else | ||
2365 | { | ||
2366 | if (Body != IntPtr.Zero) | ||
2367 | { | ||
2368 | if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) | ||
2369 | { | ||
2370 | _mesh = null; | ||
2371 | changeadd(2f); | ||
2372 | } | ||
2373 | if (childPrim) | ||
2374 | { | ||
2375 | if (_parent != null) | ||
2376 | { | ||
2377 | OdePrim parent = (OdePrim)_parent; | ||
2378 | parent.ChildDelink(this); | ||
2379 | } | ||
2380 | } | ||
2381 | else | ||
2382 | { | ||
2383 | disableBody(); | ||
2384 | } | ||
2385 | } | ||
2386 | } | ||
2387 | |||
2388 | changeSelectedStatus(); | ||
2389 | |||
2390 | resetCollisionAccounting(); | ||
2391 | m_taintPhysics = m_isphysical; | ||
2392 | } | ||
2393 | |||
2394 | public void changesize(float timestamp) | ||
2395 | { | ||
2396 | |||
2397 | string oldname = _parent_scene.geom_name_map[prim_geom]; | ||
2398 | |||
2399 | if (_size.X <= 0) _size.X = 0.01f; | ||
2400 | if (_size.Y <= 0) _size.Y = 0.01f; | ||
2401 | if (_size.Z <= 0) _size.Z = 0.01f; | ||
2402 | |||
2403 | // Cleanup of old prim geometry | ||
2404 | if (_mesh != null) | ||
2405 | { | ||
2406 | // Cleanup meshing here | ||
2407 | } | ||
2408 | //kill body to rebuild | ||
2409 | if (IsPhysical && Body != IntPtr.Zero) | ||
2410 | { | ||
2411 | if (childPrim) | ||
2412 | { | ||
2413 | if (_parent != null) | ||
2414 | { | ||
2415 | OdePrim parent = (OdePrim)_parent; | ||
2416 | parent.ChildDelink(this); | ||
2417 | } | ||
2418 | } | ||
2419 | else | ||
2420 | { | ||
2421 | disableBody(); | ||
2422 | } | ||
2423 | } | ||
2424 | if (d.SpaceQuery(m_targetSpace, prim_geom)) | ||
2425 | { | ||
2426 | _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
2427 | d.SpaceRemove(m_targetSpace, prim_geom); | ||
2428 | } | ||
2429 | // we don't need to do space calculation because the client sends a position update also. | ||
2430 | |||
2431 | // Construction of new prim | ||
2432 | if (_parent_scene.needsMeshing(_pbs))// && m_meshfailed == false) | ||
2433 | { | ||
2434 | float meshlod = _parent_scene.meshSculptLOD; | ||
2435 | |||
2436 | if (IsPhysical) | ||
2437 | meshlod = _parent_scene.MeshSculptphysicalLOD; | ||
2438 | // Don't need to re-enable body.. it's done in SetMesh | ||
2439 | |||
2440 | IMesh mesh = null; | ||
2441 | |||
2442 | try | ||
2443 | { | ||
2444 | if (_parent_scene.needsMeshing(_pbs)) | ||
2445 | mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, (int)LevelOfDetail.High, true); | ||
2446 | } | ||
2447 | catch | ||
2448 | { | ||
2449 | m_meshfailed = true; | ||
2450 | mesh = null; | ||
2451 | m_log.WarnFormat("[PHYSICS]: changeSize CreateMesh fail on prim {0} at <{1},{2},{3}>", Name, _position.X, _position.Y, _position.Z); | ||
2452 | } | ||
2453 | |||
2454 | //IMesh mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, meshlod, IsPhysical); | ||
2455 | CreateGeom(m_targetSpace, mesh); | ||
2456 | } | ||
2457 | else | ||
2458 | { | ||
2459 | _mesh = null; | ||
2460 | CreateGeom(m_targetSpace, _mesh); | ||
2461 | } | ||
2462 | |||
2463 | d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); | ||
2464 | d.Quaternion myrot = new d.Quaternion(); | ||
2465 | myrot.X = _orientation.X; | ||
2466 | myrot.Y = _orientation.Y; | ||
2467 | myrot.Z = _orientation.Z; | ||
2468 | myrot.W = _orientation.W; | ||
2469 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
2470 | |||
2471 | //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z); | ||
2472 | if (IsPhysical && Body == IntPtr.Zero && !childPrim) | ||
2473 | { | ||
2474 | // Re creates body on size. | ||
2475 | // EnableBody also does setMass() | ||
2476 | enableBody(); | ||
2477 | d.BodyEnable(Body); | ||
2478 | } | ||
2479 | |||
2480 | _parent_scene.geom_name_map[prim_geom] = oldname; | ||
2481 | |||
2482 | changeSelectedStatus(); | ||
2483 | if (childPrim) | ||
2484 | { | ||
2485 | if (_parent is OdePrim) | ||
2486 | { | ||
2487 | OdePrim parent = (OdePrim)_parent; | ||
2488 | parent.ChildSetGeom(this); | ||
2489 | } | ||
2490 | } | ||
2491 | resetCollisionAccounting(); | ||
2492 | m_taintsize = _size; | ||
2493 | } | ||
2494 | |||
2495 | |||
2496 | |||
2497 | public void changefloatonwater(float timestep) | ||
2498 | { | ||
2499 | m_collidesWater = m_taintCollidesWater; | ||
2500 | |||
2501 | if (prim_geom != IntPtr.Zero) | ||
2502 | { | ||
2503 | if (m_collidesWater) | ||
2504 | { | ||
2505 | m_collisionFlags |= CollisionCategories.Water; | ||
2506 | } | ||
2507 | else | ||
2508 | { | ||
2509 | m_collisionFlags &= ~CollisionCategories.Water; | ||
2510 | } | ||
2511 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
2512 | } | ||
2513 | } | ||
2514 | |||
2515 | public void changeshape(float timestamp) | ||
2516 | { | ||
2517 | string oldname = _parent_scene.geom_name_map[prim_geom]; | ||
2518 | |||
2519 | // Cleanup of old prim geometry and Bodies | ||
2520 | if (IsPhysical && Body != IntPtr.Zero) | ||
2521 | { | ||
2522 | if (childPrim) | ||
2523 | { | ||
2524 | if (_parent != null) | ||
2525 | { | ||
2526 | OdePrim parent = (OdePrim)_parent; | ||
2527 | parent.ChildDelink(this); | ||
2528 | } | ||
2529 | } | ||
2530 | else | ||
2531 | { | ||
2532 | disableBody(); | ||
2533 | } | ||
2534 | } | ||
2535 | |||
2536 | |||
2537 | // we don't need to do space calculation because the client sends a position update also. | ||
2538 | if (_size.X <= 0) _size.X = 0.01f; | ||
2539 | if (_size.Y <= 0) _size.Y = 0.01f; | ||
2540 | if (_size.Z <= 0) _size.Z = 0.01f; | ||
2541 | // Construction of new prim | ||
2542 | |||
2543 | if (_parent_scene.needsMeshing(_pbs))// && m_meshfailed == false) | ||
2544 | { | ||
2545 | // Don't need to re-enable body.. it's done in SetMesh | ||
2546 | float meshlod = _parent_scene.meshSculptLOD; | ||
2547 | IMesh mesh; | ||
2548 | |||
2549 | if (IsPhysical) | ||
2550 | meshlod = _parent_scene.MeshSculptphysicalLOD; | ||
2551 | |||
2552 | bool convex; | ||
2553 | if (m_shapetype == 2) | ||
2554 | convex = true; | ||
2555 | else | ||
2556 | convex = false; | ||
2557 | |||
2558 | try | ||
2559 | { | ||
2560 | mesh = _parent_scene.mesher.CreateMesh(oldname, _pbs, _size, (int)LevelOfDetail.High, true, false,convex,false); | ||
2561 | } | ||
2562 | catch | ||
2563 | { | ||
2564 | mesh = null; | ||
2565 | m_meshfailed = true; | ||
2566 | m_log.WarnFormat("[PHYSICS]: changeAdd CreateMesh fail on prim {0} at <{1},{2},{3}>", Name, _position.X, _position.Y, _position.Z); | ||
2567 | } | ||
2568 | |||
2569 | CreateGeom(m_targetSpace, mesh); | ||
2570 | |||
2571 | // createmesh returns null when it doesn't mesh. | ||
2572 | } | ||
2573 | else | ||
2574 | { | ||
2575 | _mesh = null; | ||
2576 | CreateGeom(m_targetSpace, null); | ||
2577 | } | ||
2578 | |||
2579 | d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); | ||
2580 | d.Quaternion myrot = new d.Quaternion(); | ||
2581 | //myrot.W = _orientation.w; | ||
2582 | myrot.W = _orientation.W; | ||
2583 | myrot.X = _orientation.X; | ||
2584 | myrot.Y = _orientation.Y; | ||
2585 | myrot.Z = _orientation.Z; | ||
2586 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
2587 | |||
2588 | //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z); | ||
2589 | if (IsPhysical && Body == IntPtr.Zero) | ||
2590 | { | ||
2591 | // Re creates body on size. | ||
2592 | // EnableBody also does setMass() | ||
2593 | enableBody(); | ||
2594 | if (Body != IntPtr.Zero) | ||
2595 | { | ||
2596 | d.BodyEnable(Body); | ||
2597 | } | ||
2598 | } | ||
2599 | _parent_scene.geom_name_map[prim_geom] = oldname; | ||
2600 | |||
2601 | changeSelectedStatus(); | ||
2602 | if (childPrim) | ||
2603 | { | ||
2604 | if (_parent is OdePrim) | ||
2605 | { | ||
2606 | OdePrim parent = (OdePrim)_parent; | ||
2607 | parent.ChildSetGeom(this); | ||
2608 | } | ||
2609 | } | ||
2610 | resetCollisionAccounting(); | ||
2611 | m_taintshape = false; | ||
2612 | } | ||
2613 | |||
2614 | public void changeAddForce(float timestamp) | ||
2615 | { | ||
2616 | if (!m_isSelected) | ||
2617 | { | ||
2618 | lock (m_forcelist) | ||
2619 | { | ||
2620 | //m_log.Info("[PHYSICS]: dequeing forcelist"); | ||
2621 | if (IsPhysical) | ||
2622 | { | ||
2623 | Vector3 iforce = Vector3.Zero; | ||
2624 | int i = 0; | ||
2625 | try | ||
2626 | { | ||
2627 | for (i = 0; i < m_forcelist.Count; i++) | ||
2628 | { | ||
2629 | |||
2630 | iforce = iforce + (m_forcelist[i] * 100); | ||
2631 | } | ||
2632 | } | ||
2633 | catch (IndexOutOfRangeException) | ||
2634 | { | ||
2635 | m_forcelist = new List<Vector3>(); | ||
2636 | m_collisionscore = 0; | ||
2637 | m_interpenetrationcount = 0; | ||
2638 | m_taintforce = false; | ||
2639 | return; | ||
2640 | } | ||
2641 | catch (ArgumentOutOfRangeException) | ||
2642 | { | ||
2643 | m_forcelist = new List<Vector3>(); | ||
2644 | m_collisionscore = 0; | ||
2645 | m_interpenetrationcount = 0; | ||
2646 | m_taintforce = false; | ||
2647 | return; | ||
2648 | } | ||
2649 | d.BodyEnable(Body); | ||
2650 | |||
2651 | d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z); | ||
2652 | } | ||
2653 | m_forcelist.Clear(); | ||
2654 | } | ||
2655 | |||
2656 | m_collisionscore = 0; | ||
2657 | m_interpenetrationcount = 0; | ||
2658 | } | ||
2659 | |||
2660 | m_taintforce = false; | ||
2661 | |||
2662 | } | ||
2663 | |||
2664 | |||
2665 | |||
2666 | public void changeSetTorque(float timestamp) | ||
2667 | { | ||
2668 | if (!m_isSelected) | ||
2669 | { | ||
2670 | if (IsPhysical && Body != IntPtr.Zero) | ||
2671 | { | ||
2672 | d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z); | ||
2673 | } | ||
2674 | } | ||
2675 | |||
2676 | m_taintTorque = Vector3.Zero; | ||
2677 | } | ||
2678 | |||
2679 | public void changeAddAngularForce(float timestamp) | ||
2680 | { | ||
2681 | if (!m_isSelected) | ||
2682 | { | ||
2683 | lock (m_angularforcelist) | ||
2684 | { | ||
2685 | //m_log.Info("[PHYSICS]: dequeing forcelist"); | ||
2686 | if (IsPhysical) | ||
2687 | { | ||
2688 | Vector3 iforce = Vector3.Zero; | ||
2689 | for (int i = 0; i < m_angularforcelist.Count; i++) | ||
2690 | { | ||
2691 | iforce = iforce + (m_angularforcelist[i] * 100); | ||
2692 | } | ||
2693 | d.BodyEnable(Body); | ||
2694 | d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z); | ||
2695 | |||
2696 | } | ||
2697 | m_angularforcelist.Clear(); | ||
2698 | } | ||
2699 | |||
2700 | m_collisionscore = 0; | ||
2701 | m_interpenetrationcount = 0; | ||
2702 | } | ||
2703 | |||
2704 | m_taintaddangularforce = false; | ||
2705 | } | ||
2706 | |||
2707 | private void changevelocity(float timestep) | ||
2708 | { | ||
2709 | if (!m_isSelected) | ||
2710 | { | ||
2711 | Thread.Sleep(20); | ||
2712 | if (IsPhysical) | ||
2713 | { | ||
2714 | if (Body != IntPtr.Zero) | ||
2715 | d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z); | ||
2716 | } | ||
2717 | |||
2718 | //resetCollisionAccounting(); | ||
2719 | } | ||
2720 | m_taintVelocity = Vector3.Zero; | ||
2721 | } | ||
2722 | |||
2723 | public void UpdatePositionAndVelocity() | ||
2724 | { | ||
2725 | return; // moved to the Move () method | ||
2726 | } | ||
2727 | |||
2728 | public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj) | ||
2729 | { | ||
2730 | obj.I.M00 = pMat[0, 0]; | ||
2731 | obj.I.M01 = pMat[0, 1]; | ||
2732 | obj.I.M02 = pMat[0, 2]; | ||
2733 | obj.I.M10 = pMat[1, 0]; | ||
2734 | obj.I.M11 = pMat[1, 1]; | ||
2735 | obj.I.M12 = pMat[1, 2]; | ||
2736 | obj.I.M20 = pMat[2, 0]; | ||
2737 | obj.I.M21 = pMat[2, 1]; | ||
2738 | obj.I.M22 = pMat[2, 2]; | ||
2739 | return obj; | ||
2740 | } | ||
2741 | |||
2742 | public override void SubscribeEvents(int ms) | ||
2743 | { | ||
2744 | m_eventsubscription = ms; | ||
2745 | _parent_scene.addCollisionEventReporting(this); | ||
2746 | } | ||
2747 | |||
2748 | public override void UnSubscribeEvents() | ||
2749 | { | ||
2750 | _parent_scene.remCollisionEventReporting(this); | ||
2751 | m_eventsubscription = 0; | ||
2752 | } | ||
2753 | |||
2754 | public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) | ||
2755 | { | ||
2756 | if (CollisionEventsThisFrame == null) | ||
2757 | CollisionEventsThisFrame = new CollisionEventUpdate(); | ||
2758 | CollisionEventsThisFrame.AddCollider(CollidedWith, contact); | ||
2759 | } | ||
2760 | |||
2761 | public void SendCollisions() | ||
2762 | { | ||
2763 | if (CollisionEventsThisFrame == null) | ||
2764 | return; | ||
2765 | |||
2766 | base.SendCollisionUpdate(CollisionEventsThisFrame); | ||
2767 | |||
2768 | if (CollisionEventsThisFrame.m_objCollisionList.Count == 0) | ||
2769 | CollisionEventsThisFrame = null; | ||
2770 | else | ||
2771 | CollisionEventsThisFrame = new CollisionEventUpdate(); | ||
2772 | } | ||
2773 | |||
2774 | public override bool SubscribedEvents() | ||
2775 | { | ||
2776 | if (m_eventsubscription > 0) | ||
2777 | return true; | ||
2778 | return false; | ||
2779 | } | ||
2780 | |||
2781 | public static Matrix4 Inverse(Matrix4 pMat) | ||
2782 | { | ||
2783 | if (determinant3x3(pMat) == 0) | ||
2784 | { | ||
2785 | return Matrix4.Identity; // should probably throw an error. singluar matrix inverse not possible | ||
2786 | } | ||
2787 | |||
2788 | |||
2789 | |||
2790 | return (Adjoint(pMat) / determinant3x3(pMat)); | ||
2791 | } | ||
2792 | |||
2793 | public static Matrix4 Adjoint(Matrix4 pMat) | ||
2794 | { | ||
2795 | Matrix4 adjointMatrix = new Matrix4(); | ||
2796 | for (int i = 0; i < 4; i++) | ||
2797 | { | ||
2798 | for (int j = 0; j < 4; j++) | ||
2799 | { | ||
2800 | Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j))))); | ||
2801 | } | ||
2802 | } | ||
2803 | |||
2804 | adjointMatrix = Transpose(adjointMatrix); | ||
2805 | return adjointMatrix; | ||
2806 | } | ||
2807 | |||
2808 | public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol) | ||
2809 | { | ||
2810 | Matrix4 minor = new Matrix4(); | ||
2811 | int m = 0, n = 0; | ||
2812 | for (int i = 0; i < 4; i++) | ||
2813 | { | ||
2814 | if (i == iRow) | ||
2815 | continue; | ||
2816 | n = 0; | ||
2817 | for (int j = 0; j < 4; j++) | ||
2818 | { | ||
2819 | if (j == iCol) | ||
2820 | continue; | ||
2821 | Matrix4SetValue(ref minor, m, n, matrix[i, j]); | ||
2822 | n++; | ||
2823 | } | ||
2824 | m++; | ||
2825 | } | ||
2826 | return minor; | ||
2827 | } | ||
2828 | |||
2829 | public static Matrix4 Transpose(Matrix4 pMat) | ||
2830 | { | ||
2831 | Matrix4 transposeMatrix = new Matrix4(); | ||
2832 | for (int i = 0; i < 4; i++) | ||
2833 | for (int j = 0; j < 4; j++) | ||
2834 | Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]); | ||
2835 | return transposeMatrix; | ||
2836 | } | ||
2837 | |||
2838 | public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val) | ||
2839 | { | ||
2840 | switch (r) | ||
2841 | { | ||
2842 | case 0: | ||
2843 | switch (c) | ||
2844 | { | ||
2845 | case 0: | ||
2846 | pMat.M11 = val; | ||
2847 | break; | ||
2848 | case 1: | ||
2849 | pMat.M12 = val; | ||
2850 | break; | ||
2851 | case 2: | ||
2852 | pMat.M13 = val; | ||
2853 | break; | ||
2854 | case 3: | ||
2855 | pMat.M14 = val; | ||
2856 | break; | ||
2857 | } | ||
2858 | |||
2859 | break; | ||
2860 | case 1: | ||
2861 | switch (c) | ||
2862 | { | ||
2863 | case 0: | ||
2864 | pMat.M21 = val; | ||
2865 | break; | ||
2866 | case 1: | ||
2867 | pMat.M22 = val; | ||
2868 | break; | ||
2869 | case 2: | ||
2870 | pMat.M23 = val; | ||
2871 | break; | ||
2872 | case 3: | ||
2873 | pMat.M24 = val; | ||
2874 | break; | ||
2875 | } | ||
2876 | |||
2877 | break; | ||
2878 | case 2: | ||
2879 | switch (c) | ||
2880 | { | ||
2881 | case 0: | ||
2882 | pMat.M31 = val; | ||
2883 | break; | ||
2884 | case 1: | ||
2885 | pMat.M32 = val; | ||
2886 | break; | ||
2887 | case 2: | ||
2888 | pMat.M33 = val; | ||
2889 | break; | ||
2890 | case 3: | ||
2891 | pMat.M34 = val; | ||
2892 | break; | ||
2893 | } | ||
2894 | |||
2895 | break; | ||
2896 | case 3: | ||
2897 | switch (c) | ||
2898 | { | ||
2899 | case 0: | ||
2900 | pMat.M41 = val; | ||
2901 | break; | ||
2902 | case 1: | ||
2903 | pMat.M42 = val; | ||
2904 | break; | ||
2905 | case 2: | ||
2906 | pMat.M43 = val; | ||
2907 | break; | ||
2908 | case 3: | ||
2909 | pMat.M44 = val; | ||
2910 | break; | ||
2911 | } | ||
2912 | |||
2913 | break; | ||
2914 | } | ||
2915 | } | ||
2916 | private static float determinant3x3(Matrix4 pMat) | ||
2917 | { | ||
2918 | float det = 0; | ||
2919 | float diag1 = pMat[0, 0] * pMat[1, 1] * pMat[2, 2]; | ||
2920 | float diag2 = pMat[0, 1] * pMat[2, 1] * pMat[2, 0]; | ||
2921 | float diag3 = pMat[0, 2] * pMat[1, 0] * pMat[2, 1]; | ||
2922 | float diag4 = pMat[2, 0] * pMat[1, 1] * pMat[0, 2]; | ||
2923 | float diag5 = pMat[2, 1] * pMat[1, 2] * pMat[0, 0]; | ||
2924 | float diag6 = pMat[2, 2] * pMat[1, 0] * pMat[0, 1]; | ||
2925 | |||
2926 | det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6); | ||
2927 | return det; | ||
2928 | |||
2929 | } | ||
2930 | |||
2931 | private static void DMassCopy(ref d.Mass src, ref d.Mass dst) | ||
2932 | { | ||
2933 | dst.c.W = src.c.W; | ||
2934 | dst.c.X = src.c.X; | ||
2935 | dst.c.Y = src.c.Y; | ||
2936 | dst.c.Z = src.c.Z; | ||
2937 | dst.mass = src.mass; | ||
2938 | dst.I.M00 = src.I.M00; | ||
2939 | dst.I.M01 = src.I.M01; | ||
2940 | dst.I.M02 = src.I.M02; | ||
2941 | dst.I.M10 = src.I.M10; | ||
2942 | dst.I.M11 = src.I.M11; | ||
2943 | dst.I.M12 = src.I.M12; | ||
2944 | dst.I.M20 = src.I.M20; | ||
2945 | dst.I.M21 = src.I.M21; | ||
2946 | dst.I.M22 = src.I.M22; | ||
2947 | } | ||
2948 | |||
2949 | public override void SetMaterial(int pMaterial) | ||
2950 | { | ||
2951 | m_material = pMaterial; | ||
2952 | } | ||
2953 | |||
2954 | internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) | ||
2955 | { | ||
2956 | switch (pParam) | ||
2957 | { | ||
2958 | case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY: | ||
2959 | if (pValue < 0.01f) pValue = 0.01f; | ||
2960 | // m_angularDeflectionEfficiency = pValue; | ||
2961 | break; | ||
2962 | case Vehicle.ANGULAR_DEFLECTION_TIMESCALE: | ||
2963 | if (pValue < 0.1f) pValue = 0.1f; | ||
2964 | // m_angularDeflectionTimescale = pValue; | ||
2965 | break; | ||
2966 | case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: | ||
2967 | if (pValue < 0.3f) pValue = 0.3f; | ||
2968 | m_angularMotorDecayTimescale = pValue; | ||
2969 | break; | ||
2970 | case Vehicle.ANGULAR_MOTOR_TIMESCALE: | ||
2971 | if (pValue < 0.3f) pValue = 0.3f; | ||
2972 | m_angularMotorTimescale = pValue; | ||
2973 | break; | ||
2974 | case Vehicle.BANKING_EFFICIENCY: | ||
2975 | if (pValue < 0.01f) pValue = 0.01f; | ||
2976 | // m_bankingEfficiency = pValue; | ||
2977 | break; | ||
2978 | case Vehicle.BANKING_MIX: | ||
2979 | if (pValue < 0.01f) pValue = 0.01f; | ||
2980 | // m_bankingMix = pValue; | ||
2981 | break; | ||
2982 | case Vehicle.BANKING_TIMESCALE: | ||
2983 | if (pValue < 0.01f) pValue = 0.01f; | ||
2984 | // m_bankingTimescale = pValue; | ||
2985 | break; | ||
2986 | case Vehicle.BUOYANCY: | ||
2987 | if (pValue < -1f) pValue = -1f; | ||
2988 | if (pValue > 1f) pValue = 1f; | ||
2989 | m_VehicleBuoyancy = pValue; | ||
2990 | break; | ||
2991 | // case Vehicle.HOVER_EFFICIENCY: | ||
2992 | // if (pValue < 0f) pValue = 0f; | ||
2993 | // if (pValue > 1f) pValue = 1f; | ||
2994 | // m_VhoverEfficiency = pValue; | ||
2995 | // break; | ||
2996 | case Vehicle.HOVER_HEIGHT: | ||
2997 | m_VhoverHeight = pValue; | ||
2998 | break; | ||
2999 | case Vehicle.HOVER_TIMESCALE: | ||
3000 | if (pValue < 0.1f) pValue = 0.1f; | ||
3001 | m_VhoverTimescale = pValue; | ||
3002 | break; | ||
3003 | case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: | ||
3004 | if (pValue < 0.01f) pValue = 0.01f; | ||
3005 | // m_linearDeflectionEfficiency = pValue; | ||
3006 | break; | ||
3007 | case Vehicle.LINEAR_DEFLECTION_TIMESCALE: | ||
3008 | if (pValue < 0.01f) pValue = 0.01f; | ||
3009 | // m_linearDeflectionTimescale = pValue; | ||
3010 | break; | ||
3011 | case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: | ||
3012 | if (pValue < 0.3f) pValue = 0.3f; | ||
3013 | m_linearMotorDecayTimescale = pValue; | ||
3014 | break; | ||
3015 | case Vehicle.LINEAR_MOTOR_TIMESCALE: | ||
3016 | if (pValue < 0.1f) pValue = 0.1f; | ||
3017 | m_linearMotorTimescale = pValue; | ||
3018 | break; | ||
3019 | case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: | ||
3020 | if (pValue < 0.1f) pValue = 0.1f; // Less goes unstable | ||
3021 | if (pValue > 1.0f) pValue = 1.0f; | ||
3022 | m_verticalAttractionEfficiency = pValue; | ||
3023 | break; | ||
3024 | case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: | ||
3025 | if (pValue < 0.1f) pValue = 0.1f; | ||
3026 | m_verticalAttractionTimescale = pValue; | ||
3027 | break; | ||
3028 | |||
3029 | // These are vector properties but the engine lets you use a single float value to | ||
3030 | // set all of the components to the same value | ||
3031 | case Vehicle.ANGULAR_FRICTION_TIMESCALE: | ||
3032 | if (pValue > 30f) pValue = 30f; | ||
3033 | if (pValue < 0.1f) pValue = 0.1f; | ||
3034 | m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); | ||
3035 | break; | ||
3036 | case Vehicle.ANGULAR_MOTOR_DIRECTION: | ||
3037 | m_angularMotorDirection = new Vector3(pValue, pValue, pValue); | ||
3038 | UpdateAngDecay(); | ||
3039 | break; | ||
3040 | case Vehicle.LINEAR_FRICTION_TIMESCALE: | ||
3041 | if (pValue < 0.1f) pValue = 0.1f; | ||
3042 | m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); | ||
3043 | break; | ||
3044 | case Vehicle.LINEAR_MOTOR_DIRECTION: | ||
3045 | m_linearMotorDirection = new Vector3(pValue, pValue, pValue); | ||
3046 | UpdateLinDecay(); | ||
3047 | break; | ||
3048 | case Vehicle.LINEAR_MOTOR_OFFSET: | ||
3049 | // m_linearMotorOffset = new Vector3(pValue, pValue, pValue); | ||
3050 | break; | ||
3051 | |||
3052 | } | ||
3053 | |||
3054 | }//end ProcessFloatVehicleParam | ||
3055 | |||
3056 | internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue) | ||
3057 | { | ||
3058 | switch (pParam) | ||
3059 | { | ||
3060 | case Vehicle.ANGULAR_FRICTION_TIMESCALE: | ||
3061 | if (pValue.X > 30f) pValue.X = 30f; | ||
3062 | if (pValue.X < 0.1f) pValue.X = 0.1f; | ||
3063 | if (pValue.Y > 30f) pValue.Y = 30f; | ||
3064 | if (pValue.Y < 0.1f) pValue.Y = 0.1f; | ||
3065 | if (pValue.Z > 30f) pValue.Z = 30f; | ||
3066 | if (pValue.Z < 0.1f) pValue.Z = 0.1f; | ||
3067 | m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
3068 | break; | ||
3069 | case Vehicle.ANGULAR_MOTOR_DIRECTION: | ||
3070 | m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
3071 | // Limit requested angular speed to 2 rps= 4 pi rads/sec | ||
3072 | if (m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f; | ||
3073 | if (m_angularMotorDirection.X < -12.56f) m_angularMotorDirection.X = -12.56f; | ||
3074 | if (m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f; | ||
3075 | if (m_angularMotorDirection.Y < -12.56f) m_angularMotorDirection.Y = -12.56f; | ||
3076 | if (m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f; | ||
3077 | if (m_angularMotorDirection.Z < -12.56f) m_angularMotorDirection.Z = -12.56f; | ||
3078 | UpdateAngDecay(); | ||
3079 | break; | ||
3080 | case Vehicle.LINEAR_FRICTION_TIMESCALE: | ||
3081 | if (pValue.X < 0.1f) pValue.X = 0.1f; | ||
3082 | if (pValue.Y < 0.1f) pValue.Y = 0.1f; | ||
3083 | if (pValue.Z < 0.1f) pValue.Z = 0.1f; | ||
3084 | m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
3085 | break; | ||
3086 | case Vehicle.LINEAR_MOTOR_DIRECTION: | ||
3087 | m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); // velocity requested by LSL, for max limiting | ||
3088 | UpdateLinDecay(); | ||
3089 | break; | ||
3090 | case Vehicle.LINEAR_MOTOR_OFFSET: | ||
3091 | // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
3092 | break; | ||
3093 | } | ||
3094 | |||
3095 | }//end ProcessVectorVehicleParam | ||
3096 | |||
3097 | internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) | ||
3098 | { | ||
3099 | switch (pParam) | ||
3100 | { | ||
3101 | case Vehicle.REFERENCE_FRAME: | ||
3102 | // m_referenceFrame = pValue; | ||
3103 | break; | ||
3104 | } | ||
3105 | |||
3106 | }//end ProcessRotationVehicleParam | ||
3107 | |||
3108 | internal void ProcessVehicleFlags(int pParam, bool remove) | ||
3109 | { | ||
3110 | if (remove) | ||
3111 | { | ||
3112 | m_flags &= ~((VehicleFlag)pParam); | ||
3113 | } | ||
3114 | else | ||
3115 | { | ||
3116 | m_flags |= (VehicleFlag)pParam; | ||
3117 | } | ||
3118 | } | ||
3119 | |||
3120 | internal void ProcessTypeChange(Vehicle pType) | ||
3121 | { | ||
3122 | // Set Defaults For Type | ||
3123 | m_type = pType; | ||
3124 | switch (pType) | ||
3125 | { | ||
3126 | case Vehicle.TYPE_SLED: | ||
3127 | m_linearFrictionTimescale = new Vector3(30, 1, 1000); | ||
3128 | m_angularFrictionTimescale = new Vector3(30, 30, 30); | ||
3129 | // m_lLinMotorVel = Vector3.Zero; | ||
3130 | m_linearMotorTimescale = 1000; | ||
3131 | m_linearMotorDecayTimescale = 120; | ||
3132 | m_angularMotorDirection = Vector3.Zero; | ||
3133 | m_angularMotorDVel = Vector3.Zero; | ||
3134 | m_angularMotorTimescale = 1000; | ||
3135 | m_angularMotorDecayTimescale = 120; | ||
3136 | m_VhoverHeight = 0; | ||
3137 | // m_VhoverEfficiency = 1; | ||
3138 | m_VhoverTimescale = 10; | ||
3139 | m_VehicleBuoyancy = 0; | ||
3140 | // m_linearDeflectionEfficiency = 1; | ||
3141 | // m_linearDeflectionTimescale = 1; | ||
3142 | // m_angularDeflectionEfficiency = 1; | ||
3143 | // m_angularDeflectionTimescale = 1000; | ||
3144 | // m_bankingEfficiency = 0; | ||
3145 | // m_bankingMix = 1; | ||
3146 | // m_bankingTimescale = 10; | ||
3147 | // m_referenceFrame = Quaternion.Identity; | ||
3148 | m_flags &= | ||
3149 | ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | | ||
3150 | VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); | ||
3151 | m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP); | ||
3152 | break; | ||
3153 | case Vehicle.TYPE_CAR: | ||
3154 | m_linearFrictionTimescale = new Vector3(100, 2, 1000); | ||
3155 | m_angularFrictionTimescale = new Vector3(30, 30, 30); // was 1000, but sl max frict time is 30. | ||
3156 | // m_lLinMotorVel = Vector3.Zero; | ||
3157 | m_linearMotorTimescale = 1; | ||
3158 | m_linearMotorDecayTimescale = 60; | ||
3159 | m_angularMotorDirection = Vector3.Zero; | ||
3160 | m_angularMotorDVel = Vector3.Zero; | ||
3161 | m_angularMotorTimescale = 1; | ||
3162 | m_angularMotorDecayTimescale = 0.8f; | ||
3163 | m_VhoverHeight = 0; | ||
3164 | // m_VhoverEfficiency = 0; | ||
3165 | m_VhoverTimescale = 1000; | ||
3166 | m_VehicleBuoyancy = 0; | ||
3167 | // // m_linearDeflectionEfficiency = 1; | ||
3168 | // // m_linearDeflectionTimescale = 2; | ||
3169 | // // m_angularDeflectionEfficiency = 0; | ||
3170 | // m_angularDeflectionTimescale = 10; | ||
3171 | m_verticalAttractionEfficiency = 1f; | ||
3172 | m_verticalAttractionTimescale = 10f; | ||
3173 | // m_bankingEfficiency = -0.2f; | ||
3174 | // m_bankingMix = 1; | ||
3175 | // m_bankingTimescale = 1; | ||
3176 | // m_referenceFrame = Quaternion.Identity; | ||
3177 | m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); | ||
3178 | m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY | | ||
3179 | VehicleFlag.LIMIT_MOTOR_UP); | ||
3180 | break; | ||
3181 | case Vehicle.TYPE_BOAT: | ||
3182 | m_linearFrictionTimescale = new Vector3(10, 3, 2); | ||
3183 | m_angularFrictionTimescale = new Vector3(10, 10, 10); | ||
3184 | // m_lLinMotorVel = Vector3.Zero; | ||
3185 | m_linearMotorTimescale = 5; | ||
3186 | m_linearMotorDecayTimescale = 60; | ||
3187 | m_angularMotorDirection = Vector3.Zero; | ||
3188 | m_angularMotorDVel = Vector3.Zero; | ||
3189 | m_angularMotorTimescale = 4; | ||
3190 | m_angularMotorDecayTimescale = 4; | ||
3191 | m_VhoverHeight = 0; | ||
3192 | // m_VhoverEfficiency = 0.5f; | ||
3193 | m_VhoverTimescale = 2; | ||
3194 | m_VehicleBuoyancy = 1; | ||
3195 | // m_linearDeflectionEfficiency = 0.5f; | ||
3196 | // m_linearDeflectionTimescale = 3; | ||
3197 | // m_angularDeflectionEfficiency = 0.5f; | ||
3198 | // m_angularDeflectionTimescale = 5; | ||
3199 | m_verticalAttractionEfficiency = 0.5f; | ||
3200 | m_verticalAttractionTimescale = 5f; | ||
3201 | // m_bankingEfficiency = -0.3f; | ||
3202 | // m_bankingMix = 0.8f; | ||
3203 | // m_bankingTimescale = 1; | ||
3204 | // m_referenceFrame = Quaternion.Identity; | ||
3205 | m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY | | ||
3206 | VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); | ||
3207 | m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | | ||
3208 | VehicleFlag.LIMIT_MOTOR_UP); | ||
3209 | break; | ||
3210 | case Vehicle.TYPE_AIRPLANE: | ||
3211 | m_linearFrictionTimescale = new Vector3(200, 10, 5); | ||
3212 | m_angularFrictionTimescale = new Vector3(20, 20, 20); | ||
3213 | // m_lLinMotorVel = Vector3.Zero; | ||
3214 | m_linearMotorTimescale = 2; | ||
3215 | m_linearMotorDecayTimescale = 60; | ||
3216 | m_angularMotorDirection = Vector3.Zero; | ||
3217 | m_angularMotorDVel = Vector3.Zero; | ||
3218 | m_angularMotorTimescale = 4; | ||
3219 | m_angularMotorDecayTimescale = 4; | ||
3220 | m_VhoverHeight = 0; | ||
3221 | // m_VhoverEfficiency = 0.5f; | ||
3222 | m_VhoverTimescale = 1000; | ||
3223 | m_VehicleBuoyancy = 0; | ||
3224 | // m_linearDeflectionEfficiency = 0.5f; | ||
3225 | // m_linearDeflectionTimescale = 3; | ||
3226 | // m_angularDeflectionEfficiency = 1; | ||
3227 | // m_angularDeflectionTimescale = 2; | ||
3228 | m_verticalAttractionEfficiency = 0.9f; | ||
3229 | m_verticalAttractionTimescale = 2f; | ||
3230 | // m_bankingEfficiency = 1; | ||
3231 | // m_bankingMix = 0.7f; | ||
3232 | // m_bankingTimescale = 2; | ||
3233 | // m_referenceFrame = Quaternion.Identity; | ||
3234 | m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | | ||
3235 | VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP); | ||
3236 | m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); | ||
3237 | break; | ||
3238 | case Vehicle.TYPE_BALLOON: | ||
3239 | m_linearFrictionTimescale = new Vector3(5, 5, 5); | ||
3240 | m_angularFrictionTimescale = new Vector3(10, 10, 10); | ||
3241 | m_linearMotorTimescale = 5; | ||
3242 | m_linearMotorDecayTimescale = 60; | ||
3243 | m_angularMotorDirection = Vector3.Zero; | ||
3244 | m_angularMotorDVel = Vector3.Zero; | ||
3245 | m_angularMotorTimescale = 6; | ||
3246 | m_angularMotorDecayTimescale = 10; | ||
3247 | m_VhoverHeight = 5; | ||
3248 | // m_VhoverEfficiency = 0.8f; | ||
3249 | m_VhoverTimescale = 10; | ||
3250 | m_VehicleBuoyancy = 1; | ||
3251 | // m_linearDeflectionEfficiency = 0; | ||
3252 | // m_linearDeflectionTimescale = 5; | ||
3253 | // m_angularDeflectionEfficiency = 0; | ||
3254 | // m_angularDeflectionTimescale = 5; | ||
3255 | m_verticalAttractionEfficiency = 1f; | ||
3256 | m_verticalAttractionTimescale = 100f; | ||
3257 | // m_bankingEfficiency = 0; | ||
3258 | // m_bankingMix = 0.7f; | ||
3259 | // m_bankingTimescale = 5; | ||
3260 | // m_referenceFrame = Quaternion.Identity; | ||
3261 | m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | | ||
3262 | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP); | ||
3263 | m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); | ||
3264 | break; | ||
3265 | |||
3266 | } | ||
3267 | }//end SetDefaultsForType | ||
3268 | |||
3269 | internal void Enable(IntPtr pBody, OdeScene pParentScene) | ||
3270 | { | ||
3271 | if (m_type == Vehicle.TYPE_NONE) | ||
3272 | return; | ||
3273 | |||
3274 | m_body = pBody; | ||
3275 | } | ||
3276 | |||
3277 | |||
3278 | internal void Halt() | ||
3279 | { // Kill all motions, when non-physical | ||
3280 | // m_linearMotorDirection = Vector3.Zero; | ||
3281 | m_lLinMotorDVel = Vector3.Zero; | ||
3282 | m_lLinObjectVel = Vector3.Zero; | ||
3283 | m_wLinObjectVel = Vector3.Zero; | ||
3284 | m_angularMotorDirection = Vector3.Zero; | ||
3285 | m_lastAngularVelocity = Vector3.Zero; | ||
3286 | m_angularMotorDVel = Vector3.Zero; | ||
3287 | _acceleration = Vector3.Zero; | ||
3288 | } | ||
3289 | |||
3290 | private void UpdateLinDecay() | ||
3291 | { | ||
3292 | m_lLinMotorDVel.X = m_linearMotorDirection.X; | ||
3293 | m_lLinMotorDVel.Y = m_linearMotorDirection.Y; | ||
3294 | m_lLinMotorDVel.Z = m_linearMotorDirection.Z; | ||
3295 | } // else let the motor decay on its own | ||
3296 | |||
3297 | private void UpdateAngDecay() | ||
3298 | { | ||
3299 | m_angularMotorDVel.X = m_angularMotorDirection.X; | ||
3300 | m_angularMotorDVel.Y = m_angularMotorDirection.Y; | ||
3301 | m_angularMotorDVel.Z = m_angularMotorDirection.Z; | ||
3302 | } // else let the motor decay on its own | ||
3303 | |||
3304 | public void Move(float timestep) | ||
3305 | { | ||
3306 | float fx = 0; | ||
3307 | float fy = 0; | ||
3308 | float fz = 0; | ||
3309 | Vector3 linvel; // velocity applied, including any reversal | ||
3310 | |||
3311 | // If geomCrossingFailuresBeforeOutofbounds is set to 0 in OpenSim.ini then phys objects bounce off region borders. | ||
3312 | // This is a temp patch until proper region crossing is developed. | ||
3313 | |||
3314 | |||
3315 | if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim && !m_outofBounds) // Only move root prims. | ||
3316 | { | ||
3317 | // Old public void UpdatePositionAndVelocity(), more accuratley calculated here | ||
3318 | bool lastZeroFlag = _zeroFlag; // was it stopped | ||
3319 | |||
3320 | d.Vector3 vec = d.BodyGetPosition(Body); | ||
3321 | Vector3 l_position = Vector3.Zero; | ||
3322 | l_position.X = vec.X; | ||
3323 | l_position.Y = vec.Y; | ||
3324 | l_position.Z = vec.Z; | ||
3325 | m_lastposition = _position; | ||
3326 | _position = l_position; | ||
3327 | |||
3328 | d.Quaternion ori = d.BodyGetQuaternion(Body); | ||
3329 | // Quaternion l_orientation = Quaternion.Identity; | ||
3330 | _orientation.X = ori.X; | ||
3331 | _orientation.Y = ori.Y; | ||
3332 | _orientation.Z = ori.Z; | ||
3333 | _orientation.W = ori.W; | ||
3334 | m_lastorientation = _orientation; | ||
3335 | |||
3336 | d.Vector3 vel = d.BodyGetLinearVel(Body); | ||
3337 | m_lastVelocity = _velocity; | ||
3338 | _velocity.X = vel.X; | ||
3339 | _velocity.Y = vel.Y; | ||
3340 | _velocity.Z = vel.Z; | ||
3341 | _acceleration = ((_velocity - m_lastVelocity) / timestep); | ||
3342 | |||
3343 | d.Vector3 torque = d.BodyGetTorque(Body); | ||
3344 | _torque = new Vector3(torque.X, torque.Y, torque.Z); | ||
3345 | |||
3346 | |||
3347 | if (_position.X < 0f || _position.X > _parent_scene.WorldExtents.X | ||
3348 | || _position.Y < 0f || _position.Y > _parent_scene.WorldExtents.Y | ||
3349 | ) | ||
3350 | { | ||
3351 | // we are outside current region | ||
3352 | // clip position to a stop just outside region and stop it only internally | ||
3353 | // do it only once using m_crossingfailures as control | ||
3354 | _position.X = Util.Clip(l_position.X, -0.2f, _parent_scene.WorldExtents.X + .2f); | ||
3355 | _position.Y = Util.Clip(l_position.Y, -0.2f, _parent_scene.WorldExtents.Y + .2f); | ||
3356 | _position.Z = Util.Clip(l_position.Z, -100f, 50000f); | ||
3357 | d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); | ||
3358 | d.BodySetLinearVel(Body, 0, 0, 0); | ||
3359 | m_outofBounds = true; | ||
3360 | base.RequestPhysicsterseUpdate(); | ||
3361 | return; | ||
3362 | } | ||
3363 | |||
3364 | base.RequestPhysicsterseUpdate(); | ||
3365 | |||
3366 | if (l_position.Z < 0) | ||
3367 | { | ||
3368 | // This is so prim that get lost underground don't fall forever and suck up | ||
3369 | // | ||
3370 | // Sim resources and memory. | ||
3371 | // Disables the prim's movement physics.... | ||
3372 | // It's a hack and will generate a console message if it fails. | ||
3373 | |||
3374 | //IsPhysical = false; | ||
3375 | if (_parent == null) base.RaiseOutOfBounds(_position); | ||
3376 | |||
3377 | |||
3378 | _acceleration.X = 0; // This stuff may stop client display but it has no | ||
3379 | _acceleration.Y = 0; // effect on the object in phys engine! | ||
3380 | _acceleration.Z = 0; | ||
3381 | |||
3382 | _velocity.X = 0; | ||
3383 | _velocity.Y = 0; | ||
3384 | _velocity.Z = 0; | ||
3385 | m_lastVelocity = Vector3.Zero; | ||
3386 | m_rotationalVelocity.X = 0; | ||
3387 | m_rotationalVelocity.Y = 0; | ||
3388 | m_rotationalVelocity.Z = 0; | ||
3389 | |||
3390 | if (_parent == null) base.RequestPhysicsterseUpdate(); | ||
3391 | |||
3392 | m_throttleUpdates = false; | ||
3393 | throttleCounter = 0; | ||
3394 | _zeroFlag = true; | ||
3395 | //outofBounds = true; | ||
3396 | } // end neg Z check | ||
3397 | |||
3398 | // Is it moving? | ||
3399 | /* if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) | ||
3400 | && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) | ||
3401 | && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) */ | ||
3402 | if ((Vector3.Mag(_velocity) < 0.01) && // moving very slowly | ||
3403 | (Vector3.Mag(_velocity) < Vector3.Mag(m_lastVelocity)) && // decelerating | ||
3404 | (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, _orientation)) < 0.0001)) // spinning very slowly | ||
3405 | { | ||
3406 | _zeroFlag = true; | ||
3407 | m_throttleUpdates = false; | ||
3408 | } | ||
3409 | else | ||
3410 | { | ||
3411 | //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString()); | ||
3412 | _zeroFlag = false; | ||
3413 | m_lastUpdateSent = false; | ||
3414 | //m_throttleUpdates = false; | ||
3415 | } | ||
3416 | |||
3417 | if (_zeroFlag) | ||
3418 | { // Its stopped | ||
3419 | _velocity.X = 0.0f; | ||
3420 | _velocity.Y = 0.0f; | ||
3421 | // _velocity.Z = 0.0f; | ||
3422 | |||
3423 | _acceleration.X = 0; | ||
3424 | _acceleration.Y = 0; | ||
3425 | // _acceleration.Z = 0; | ||
3426 | |||
3427 | m_rotationalVelocity.X = 0; | ||
3428 | m_rotationalVelocity.Y = 0; | ||
3429 | m_rotationalVelocity.Z = 0; | ||
3430 | // Stop it in the phys engine | ||
3431 | d.BodySetLinearVel(Body, 0.0f, 0.0f, _velocity.Z); | ||
3432 | d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f); | ||
3433 | d.BodySetForce(Body, 0f, 0f, 0f); | ||
3434 | |||
3435 | if (!m_lastUpdateSent) | ||
3436 | { | ||
3437 | m_throttleUpdates = false; | ||
3438 | throttleCounter = 0; | ||
3439 | if (_parent == null) | ||
3440 | { | ||
3441 | base.RequestPhysicsterseUpdate(); | ||
3442 | } | ||
3443 | |||
3444 | m_lastUpdateSent = true; | ||
3445 | } | ||
3446 | } | ||
3447 | else | ||
3448 | { // Its moving | ||
3449 | if (lastZeroFlag != _zeroFlag) | ||
3450 | { | ||
3451 | if (_parent == null) | ||
3452 | { | ||
3453 | base.RequestPhysicsterseUpdate(); | ||
3454 | } | ||
3455 | } | ||
3456 | m_lastUpdateSent = false; | ||
3457 | if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) | ||
3458 | { | ||
3459 | if (_parent == null) | ||
3460 | { | ||
3461 | base.RequestPhysicsterseUpdate(); | ||
3462 | } | ||
3463 | } | ||
3464 | else | ||
3465 | { | ||
3466 | throttleCounter++; | ||
3467 | } | ||
3468 | } | ||
3469 | m_lastposition = l_position; | ||
3470 | |||
3471 | /// End UpdatePositionAndVelocity insert | ||
3472 | |||
3473 | |||
3474 | // Rotation lock ===================================== | ||
3475 | if (m_rotateEnableUpdate) | ||
3476 | { | ||
3477 | // Snapshot current angles, set up Amotor(s) | ||
3478 | m_rotateEnableUpdate = false; | ||
3479 | m_rotateEnable = m_rotateEnableRequest; | ||
3480 | //Console.WriteLine("RotEnable {0} = {1}",m_primName, m_rotateEnable); | ||
3481 | |||
3482 | if (Amotor != IntPtr.Zero) | ||
3483 | { | ||
3484 | d.JointDestroy(Amotor); | ||
3485 | Amotor = IntPtr.Zero; | ||
3486 | //Console.WriteLine("Old Amotor Destroyed"); | ||
3487 | } | ||
3488 | |||
3489 | if (!m_rotateEnable.ApproxEquals(Vector3.One, 0.003f)) | ||
3490 | { // not all are enabled | ||
3491 | d.Quaternion r = d.BodyGetQuaternion(Body); | ||
3492 | Quaternion locrot = new Quaternion(r.X, r.Y, r.Z, r.W); | ||
3493 | // extract the axes vectors | ||
3494 | Vector3 vX = new Vector3(1f, 0f, 0f); | ||
3495 | Vector3 vY = new Vector3(0f, 1f, 0f); | ||
3496 | Vector3 vZ = new Vector3(0f, 0f, 1f); | ||
3497 | vX = vX * locrot; | ||
3498 | vY = vY * locrot; | ||
3499 | vZ = vZ * locrot; | ||
3500 | // snapshot the current angle vectors | ||
3501 | m_lockX = vX; | ||
3502 | m_lockY = vY; | ||
3503 | m_lockZ = vZ; | ||
3504 | // m_lockRot = locrot; | ||
3505 | Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); | ||
3506 | d.JointAttach(Amotor, Body, IntPtr.Zero); | ||
3507 | d.JointSetAMotorMode(Amotor, 0); // User mode?? | ||
3508 | //Console.WriteLine("New Amotor Created for {0}", m_primName); | ||
3509 | |||
3510 | float axisnum = 3; // how many to lock | ||
3511 | axisnum = (axisnum - (m_rotateEnable.X + m_rotateEnable.Y + m_rotateEnable.Z)); | ||
3512 | d.JointSetAMotorNumAxes(Amotor, (int)axisnum); | ||
3513 | //Console.WriteLine("AxisNum={0}",(int)axisnum); | ||
3514 | |||
3515 | int i = 0; | ||
3516 | |||
3517 | if (m_rotateEnable.X == 0) | ||
3518 | { | ||
3519 | d.JointSetAMotorAxis(Amotor, i, 0, m_lockX.X, m_lockX.Y, m_lockX.Z); | ||
3520 | //Console.WriteLine("AxisX {0} set to {1}", i, m_lockX); | ||
3521 | i++; | ||
3522 | } | ||
3523 | |||
3524 | if (m_rotateEnable.Y == 0) | ||
3525 | { | ||
3526 | d.JointSetAMotorAxis(Amotor, i, 0, m_lockY.X, m_lockY.Y, m_lockY.Z); | ||
3527 | //Console.WriteLine("AxisY {0} set to {1}", i, m_lockY); | ||
3528 | i++; | ||
3529 | } | ||
3530 | |||
3531 | if (m_rotateEnable.Z == 0) | ||
3532 | { | ||
3533 | d.JointSetAMotorAxis(Amotor, i, 0, m_lockZ.X, m_lockZ.Y, m_lockZ.Z); | ||
3534 | //Console.WriteLine("AxisZ {0} set to {1}", i, m_lockZ); | ||
3535 | i++; | ||
3536 | } | ||
3537 | |||
3538 | // These lowstops and high stops are effectively (no wiggle room) | ||
3539 | d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, 0f); | ||
3540 | d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, 0f); | ||
3541 | d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, 0f); | ||
3542 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f); | ||
3543 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); | ||
3544 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f); | ||
3545 | d.JointSetAMotorParam(Amotor, (int)dParam.Vel, 0f); | ||
3546 | d.JointSetAMotorParam(Amotor, (int)dParam.Vel3, 0f); | ||
3547 | d.JointSetAMotorParam(Amotor, (int)dParam.Vel2, 0f); | ||
3548 | d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM, 0f); | ||
3549 | d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM3, 0f); | ||
3550 | d.JointSetAMotorParam(Amotor, (int)dParam.StopCFM2, 0f); | ||
3551 | } // else none are locked | ||
3552 | } // end Rotation Update | ||
3553 | |||
3554 | |||
3555 | // VEHICLE processing ========================================== | ||
3556 | if (m_type != Vehicle.TYPE_NONE) | ||
3557 | { | ||
3558 | // get body attitude | ||
3559 | d.Quaternion rot = d.BodyGetQuaternion(Body); | ||
3560 | Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object | ||
3561 | Quaternion irotq = Quaternion.Inverse(rotq); | ||
3562 | |||
3563 | // VEHICLE Linear Motion | ||
3564 | d.Vector3 velnow = d.BodyGetLinearVel(Body); // this is in world frame | ||
3565 | Vector3 vel_now = new Vector3(velnow.X, velnow.Y, velnow.Z); | ||
3566 | m_lLinObjectVel = vel_now * irotq; | ||
3567 | if (m_linearMotorDecayTimescale < 300.0f) //setting of 300 or more disables decay rate | ||
3568 | { | ||
3569 | if (Vector3.Mag(m_lLinMotorDVel) < 1.0f) | ||
3570 | { | ||
3571 | float decayfactor = m_linearMotorDecayTimescale / timestep; | ||
3572 | Vector3 decayAmount = (m_lLinMotorDVel / decayfactor); | ||
3573 | m_lLinMotorDVel -= decayAmount; | ||
3574 | } | ||
3575 | else | ||
3576 | { | ||
3577 | float decayfactor = 3.0f - (0.57f * (float)Math.Log((double)(m_linearMotorDecayTimescale))); | ||
3578 | Vector3 decel = Vector3.Normalize(m_lLinMotorDVel) * decayfactor * timestep; | ||
3579 | m_lLinMotorDVel -= decel; | ||
3580 | } | ||
3581 | if (m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) | ||
3582 | { | ||
3583 | m_lLinMotorDVel = Vector3.Zero; | ||
3584 | } | ||
3585 | |||
3586 | /* else | ||
3587 | { | ||
3588 | if (Math.Abs(m_lLinMotorDVel.X) < Math.Abs(m_lLinObjectVel.X)) m_lLinObjectVel.X = m_lLinMotorDVel.X; | ||
3589 | if (Math.Abs(m_lLinMotorDVel.Y) < Math.Abs(m_lLinObjectVel.Y)) m_lLinObjectVel.Y = m_lLinMotorDVel.Y; | ||
3590 | if (Math.Abs(m_lLinMotorDVel.Z) < Math.Abs(m_lLinObjectVel.Z)) m_lLinObjectVel.Z = m_lLinMotorDVel.Z; | ||
3591 | } */ | ||
3592 | } // end linear motor decay | ||
3593 | |||
3594 | if ((!m_lLinMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (!m_lLinObjectVel.ApproxEquals(Vector3.Zero, 0.01f))) | ||
3595 | { | ||
3596 | if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); | ||
3597 | if (m_linearMotorTimescale < 300.0f) | ||
3598 | { | ||
3599 | Vector3 attack_error = m_lLinMotorDVel - m_lLinObjectVel; | ||
3600 | float linfactor = m_linearMotorTimescale / timestep; | ||
3601 | Vector3 attackAmount = (attack_error / linfactor) * 1.3f; | ||
3602 | m_lLinObjectVel += attackAmount; | ||
3603 | } | ||
3604 | if (m_linearFrictionTimescale.X < 300.0f) | ||
3605 | { | ||
3606 | float fricfactor = m_linearFrictionTimescale.X / timestep; | ||
3607 | float fricX = m_lLinObjectVel.X / fricfactor; | ||
3608 | m_lLinObjectVel.X -= fricX; | ||
3609 | } | ||
3610 | if (m_linearFrictionTimescale.Y < 300.0f) | ||
3611 | { | ||
3612 | float fricfactor = m_linearFrictionTimescale.Y / timestep; | ||
3613 | float fricY = m_lLinObjectVel.Y / fricfactor; | ||
3614 | m_lLinObjectVel.Y -= fricY; | ||
3615 | } | ||
3616 | if (m_linearFrictionTimescale.Z < 300.0f) | ||
3617 | { | ||
3618 | float fricfactor = m_linearFrictionTimescale.Z / timestep; | ||
3619 | float fricZ = m_lLinObjectVel.Z / fricfactor; | ||
3620 | m_lLinObjectVel.Z -= fricZ; | ||
3621 | } | ||
3622 | } | ||
3623 | m_wLinObjectVel = m_lLinObjectVel * rotq; | ||
3624 | |||
3625 | // Gravity and Buoyancy | ||
3626 | Vector3 grav = Vector3.Zero; | ||
3627 | if (m_VehicleBuoyancy < 1.0f) | ||
3628 | { | ||
3629 | // There is some gravity, make a gravity force vector | ||
3630 | // that is applied after object velocity. | ||
3631 | d.Mass objMass; | ||
3632 | d.BodyGetMass(Body, out objMass); | ||
3633 | // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; | ||
3634 | grav.Z = _parent_scene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy); // Applied later as a force | ||
3635 | } // else its 1.0, no gravity. | ||
3636 | |||
3637 | // Hovering | ||
3638 | if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) | ||
3639 | { | ||
3640 | // We should hover, get the target height | ||
3641 | d.Vector3 pos = d.BodyGetPosition(Body); | ||
3642 | if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY) | ||
3643 | { | ||
3644 | m_VhoverTargetHeight = _parent_scene.GetWaterLevel() + m_VhoverHeight; | ||
3645 | } | ||
3646 | else if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY) | ||
3647 | { | ||
3648 | m_VhoverTargetHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight; | ||
3649 | } | ||
3650 | else if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT) | ||
3651 | { | ||
3652 | m_VhoverTargetHeight = m_VhoverHeight; | ||
3653 | } | ||
3654 | |||
3655 | if ((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY) | ||
3656 | { | ||
3657 | // If body is aready heigher, use its height as target height | ||
3658 | if (pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z; | ||
3659 | } | ||
3660 | |||
3661 | // m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped | ||
3662 | // m_VhoverTimescale = 0f; // time to acheive height | ||
3663 | // timestep is time since last frame,in secs | ||
3664 | float herr0 = pos.Z - m_VhoverTargetHeight; | ||
3665 | // Replace Vertical speed with correction figure if significant | ||
3666 | if (Math.Abs(herr0) > 0.01f) | ||
3667 | { | ||
3668 | //? d.Mass objMass; | ||
3669 | //? d.BodyGetMass(Body, out objMass); | ||
3670 | m_wLinObjectVel.Z = -((herr0 * timestep * 50.0f) / m_VhoverTimescale); | ||
3671 | //KF: m_VhoverEfficiency is not yet implemented | ||
3672 | } | ||
3673 | else | ||
3674 | { | ||
3675 | m_wLinObjectVel.Z = 0f; | ||
3676 | } | ||
3677 | } | ||
3678 | else | ||
3679 | { // not hovering | ||
3680 | if (m_wLinObjectVel.Z == 0f) | ||
3681 | { // Gravity rules | ||
3682 | m_wLinObjectVel.Z = vel_now.Z; | ||
3683 | } // else the motor has it | ||
3684 | } | ||
3685 | linvel = m_wLinObjectVel; | ||
3686 | |||
3687 | // Vehicle Linear Motion done ======================================= | ||
3688 | // Apply velocity | ||
3689 | d.BodySetLinearVel(Body, linvel.X, linvel.Y, linvel.Z); | ||
3690 | // apply gravity force | ||
3691 | d.BodyAddForce(Body, grav.X, grav.Y, grav.Z); | ||
3692 | //if(frcount == 0) Console.WriteLine("Vel={0} Force={1}",linvel , grav); | ||
3693 | // end MoveLinear() | ||
3694 | |||
3695 | |||
3696 | // MoveAngular | ||
3697 | /* | ||
3698 | private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor | ||
3699 | |||
3700 | private float m_angularMotorTimescale = 0; // motor angular Attack rate set by LSL | ||
3701 | private float m_angularMotorDecayTimescale = 0; // motor angular Decay rate set by LSL | ||
3702 | private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular Friction set by LSL | ||
3703 | |||
3704 | private Vector3 m_angularMotorDVel = Vector3.Zero; // decayed angular motor | ||
3705 | private Vector3 m_angObjectVel = Vector3.Zero; // what was last applied to body | ||
3706 | */ | ||
3707 | //if(frcount == 0) Console.WriteLine("MoveAngular "); | ||
3708 | |||
3709 | d.Vector3 angularObjectVel = d.BodyGetAngularVel(Body); | ||
3710 | Vector3 angObjectVel = new Vector3(angularObjectVel.X, angularObjectVel.Y, angularObjectVel.Z); | ||
3711 | angObjectVel = angObjectVel * irotq; // ============ Converts to LOCAL rotation | ||
3712 | |||
3713 | //if(frcount == 0) Console.WriteLine("V0 = {0}", angObjectVel); | ||
3714 | |||
3715 | // Decay Angular Motor 1. In SL this also depends on attack rate! decay ~= 23/Attack. | ||
3716 | float atk_decayfactor = 23.0f / (m_angularMotorTimescale * timestep); | ||
3717 | m_angularMotorDVel -= m_angularMotorDVel / atk_decayfactor; | ||
3718 | // Decay Angular Motor 2. | ||
3719 | if (m_angularMotorDecayTimescale < 300.0f) | ||
3720 | { | ||
3721 | if (Vector3.Mag(m_angularMotorDVel) < 1.0f) | ||
3722 | { | ||
3723 | float decayfactor = (m_angularMotorDecayTimescale) / timestep; | ||
3724 | Vector3 decayAmount = (m_angularMotorDVel / decayfactor); | ||
3725 | m_angularMotorDVel -= decayAmount; | ||
3726 | } | ||
3727 | else | ||
3728 | { | ||
3729 | Vector3 decel = Vector3.Normalize(m_angularMotorDVel) * timestep / m_angularMotorDecayTimescale; | ||
3730 | m_angularMotorDVel -= decel; | ||
3731 | } | ||
3732 | |||
3733 | if (m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) | ||
3734 | { | ||
3735 | m_angularMotorDVel = Vector3.Zero; | ||
3736 | } | ||
3737 | else | ||
3738 | { | ||
3739 | if (Math.Abs(m_angularMotorDVel.X) < Math.Abs(angObjectVel.X)) angObjectVel.X = m_angularMotorDVel.X; | ||
3740 | if (Math.Abs(m_angularMotorDVel.Y) < Math.Abs(angObjectVel.Y)) angObjectVel.Y = m_angularMotorDVel.Y; | ||
3741 | if (Math.Abs(m_angularMotorDVel.Z) < Math.Abs(angObjectVel.Z)) angObjectVel.Z = m_angularMotorDVel.Z; | ||
3742 | } | ||
3743 | } // end decay angular motor | ||
3744 | //if(frcount == 0) Console.WriteLine("MotorDvel {0} Obj {1}", m_angularMotorDVel, angObjectVel); | ||
3745 | |||
3746 | //if(frcount == 0) Console.WriteLine("VA = {0}", angObjectVel); | ||
3747 | |||
3748 | if ((!m_angularMotorDVel.ApproxEquals(Vector3.Zero, 0.01f)) || (!angObjectVel.ApproxEquals(Vector3.Zero, 0.01f))) | ||
3749 | { // if motor or object have motion | ||
3750 | if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); | ||
3751 | |||
3752 | if (m_angularMotorTimescale < 300.0f) | ||
3753 | { | ||
3754 | Vector3 attack_error = m_angularMotorDVel - angObjectVel; | ||
3755 | float angfactor = m_angularMotorTimescale / timestep; | ||
3756 | Vector3 attackAmount = (attack_error / angfactor); | ||
3757 | angObjectVel += attackAmount; | ||
3758 | //if(frcount == 0) Console.WriteLine("Accel {0} Attk {1}",FrAaccel, attackAmount); | ||
3759 | //if(frcount == 0) Console.WriteLine("V2+= {0}", angObjectVel); | ||
3760 | } | ||
3761 | |||
3762 | angObjectVel.X -= angObjectVel.X / (m_angularFrictionTimescale.X * 0.7f / timestep); | ||
3763 | angObjectVel.Y -= angObjectVel.Y / (m_angularFrictionTimescale.Y * 0.7f / timestep); | ||
3764 | angObjectVel.Z -= angObjectVel.Z / (m_angularFrictionTimescale.Z * 0.7f / timestep); | ||
3765 | } // else no signif. motion | ||
3766 | |||
3767 | //if(frcount == 0) Console.WriteLine("Dmotor {0} Obj {1}", m_angularMotorDVel, angObjectVel); | ||
3768 | // Bank section tba | ||
3769 | // Deflection section tba | ||
3770 | //if(frcount == 0) Console.WriteLine("V3 = {0}", angObjectVel); | ||
3771 | |||
3772 | |||
3773 | /* // Rotation Axis Disables: | ||
3774 | if (!m_angularEnable.ApproxEquals(Vector3.One, 0.003f)) | ||
3775 | { | ||
3776 | if (m_angularEnable.X == 0) | ||
3777 | angObjectVel.X = 0f; | ||
3778 | if (m_angularEnable.Y == 0) | ||
3779 | angObjectVel.Y = 0f; | ||
3780 | if (m_angularEnable.Z == 0) | ||
3781 | angObjectVel.Z = 0f; | ||
3782 | } | ||
3783 | */ | ||
3784 | angObjectVel = angObjectVel * rotq; // ================ Converts to WORLD rotation | ||
3785 | |||
3786 | // Vertical attractor section | ||
3787 | Vector3 vertattr = Vector3.Zero; | ||
3788 | |||
3789 | if (m_verticalAttractionTimescale < 300) | ||
3790 | { | ||
3791 | float VAservo = 1.0f / (m_verticalAttractionTimescale * timestep); | ||
3792 | // make a vector pointing up | ||
3793 | Vector3 verterr = Vector3.Zero; | ||
3794 | verterr.Z = 1.0f; | ||
3795 | // rotate it to Body Angle | ||
3796 | verterr = verterr * rotq; | ||
3797 | // 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. | ||
3798 | // 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 | ||
3799 | // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. | ||
3800 | |||
3801 | if (verterr.Z < 0.0f) | ||
3802 | { // Deflection from vertical exceeds 90-degrees. This method will ensure stable return to | ||
3803 | // vertical, BUT for some reason a z-rotation is imparted to the object. TBI. | ||
3804 | //Console.WriteLine("InvertFlip"); | ||
3805 | verterr.X = 2.0f - verterr.X; | ||
3806 | verterr.Y = 2.0f - verterr.Y; | ||
3807 | } | ||
3808 | verterr *= 0.5f; | ||
3809 | // verterror is 0 (no error) to +/- 1 (max error at 180-deg tilt) | ||
3810 | Vector3 xyav = angObjectVel; | ||
3811 | xyav.Z = 0.0f; | ||
3812 | if ((!xyav.ApproxEquals(Vector3.Zero, 0.001f)) || (verterr.Z < 0.49f)) | ||
3813 | { | ||
3814 | // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so | ||
3815 | // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. | ||
3816 | vertattr.X = verterr.Y; | ||
3817 | vertattr.Y = -verterr.X; | ||
3818 | vertattr.Z = 0f; | ||
3819 | //if(frcount == 0) Console.WriteLine("VAerr=" + verterr); | ||
3820 | |||
3821 | // scaling appears better usingsquare-law | ||
3822 | float damped = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency; | ||
3823 | float bounce = 1.0f - damped; | ||
3824 | // 0 = crit damp, 1 = bouncy | ||
3825 | float oavz = angObjectVel.Z; // retain z velocity | ||
3826 | // time-scaled correction, which sums, therefore is bouncy: | ||
3827 | angObjectVel = (angObjectVel + (vertattr * VAservo * 0.0333f)) * bounce; | ||
3828 | // damped, good @ < 90: | ||
3829 | angObjectVel = angObjectVel + (vertattr * VAservo * 0.0667f * damped); | ||
3830 | angObjectVel.Z = oavz; | ||
3831 | //if(frcount == 0) Console.WriteLine("VA+"); | ||
3832 | //Console.WriteLine("VAttr {0} OAvel {1}", vertattr, angObjectVel); | ||
3833 | } | ||
3834 | else | ||
3835 | { | ||
3836 | // else error is very small | ||
3837 | angObjectVel.X = 0f; | ||
3838 | angObjectVel.Y = 0f; | ||
3839 | //if(frcount == 0) Console.WriteLine("VA0"); | ||
3840 | } | ||
3841 | } // else vertical attractor is off | ||
3842 | //if(frcount == 0) Console.WriteLine("V1 = {0}", angObjectVel); | ||
3843 | |||
3844 | |||
3845 | m_lastAngularVelocity = angObjectVel; | ||
3846 | // apply Angular Velocity to body | ||
3847 | d.BodySetAngularVel(Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z); | ||
3848 | //if(frcount == 0) Console.WriteLine("V4 = {0}", m_lastAngularVelocity); | ||
3849 | |||
3850 | } // end VEHICLES | ||
3851 | else | ||
3852 | { | ||
3853 | // Dyamics (NON-'VEHICLES') are dealt with here ================================================================ | ||
3854 | |||
3855 | if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009 | ||
3856 | |||
3857 | /// Dynamics Buoyancy | ||
3858 | //KF: m_buoyancy is set by llSetBuoyancy() and is for non-vehicle. | ||
3859 | // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up | ||
3860 | // NB Prims in ODE are no subject to global gravity | ||
3861 | // This should only affect gravity operations | ||
3862 | |||
3863 | float m_mass = CalculateMass(); | ||
3864 | // calculate z-force due togravity on object. | ||
3865 | fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; // force = acceleration * mass | ||
3866 | if ((m_usePID) && (m_PIDTau > 0.0f)) // Dynamics llMoveToTarget. | ||
3867 | { | ||
3868 | fz = 0; // llMoveToTarget ignores gravity. | ||
3869 | // it also ignores mass of object, and any physical resting on it. | ||
3870 | // Vector3 m_PIDTarget is where we are going | ||
3871 | // float m_PIDTau is time to get there | ||
3872 | fx = 0; | ||
3873 | fy = 0; | ||
3874 | d.Vector3 pos = d.BodyGetPosition(Body); | ||
3875 | Vector3 error = new Vector3( | ||
3876 | (m_PIDTarget.X - pos.X), | ||
3877 | (m_PIDTarget.Y - pos.Y), | ||
3878 | (m_PIDTarget.Z - pos.Z)); | ||
3879 | if (error.ApproxEquals(Vector3.Zero, 0.01f)) | ||
3880 | { // Very close, Jump there and quit move | ||
3881 | |||
3882 | d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); | ||
3883 | _target_velocity = Vector3.Zero; | ||
3884 | d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); | ||
3885 | d.BodySetForce(Body, 0f, 0f, 0f); | ||
3886 | } | ||
3887 | else | ||
3888 | { | ||
3889 | float scale = 50.0f * timestep / m_PIDTau; | ||
3890 | if ((error.ApproxEquals(Vector3.Zero, 0.5f)) && (_target_velocity != Vector3.Zero)) | ||
3891 | { | ||
3892 | // Nearby, quit update of velocity | ||
3893 | } | ||
3894 | else | ||
3895 | { // Far, calc damped velocity | ||
3896 | _target_velocity = error * scale; | ||
3897 | } | ||
3898 | d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z); | ||
3899 | } | ||
3900 | } // end PID MoveToTarget | ||
3901 | |||
3902 | |||
3903 | /// Dynamics Hover =================================================================================== | ||
3904 | // Hover PID Controller can only run if the PIDcontroller is not in use. | ||
3905 | if (m_useHoverPID && !m_usePID) | ||
3906 | { | ||
3907 | //Console.WriteLine("Hover " + m_primName); | ||
3908 | |||
3909 | // If we're using the PID controller, then we have no gravity | ||
3910 | fz = (-1 * _parent_scene.gravityz) * m_mass; | ||
3911 | |||
3912 | // no lock; for now it's only called from within Simulate() | ||
3913 | |||
3914 | // If the PID Controller isn't active then we set our force | ||
3915 | // calculating base velocity to the current position | ||
3916 | |||
3917 | if ((m_PIDTau < 1)) | ||
3918 | { | ||
3919 | PID_G = PID_G / m_PIDTau; | ||
3920 | } | ||
3921 | |||
3922 | if ((PID_G - m_PIDTau) <= 0) | ||
3923 | { | ||
3924 | PID_G = m_PIDTau + 1; | ||
3925 | } | ||
3926 | |||
3927 | |||
3928 | // Where are we, and where are we headed? | ||
3929 | d.Vector3 pos = d.BodyGetPosition(Body); | ||
3930 | // d.Vector3 vel = d.BodyGetLinearVel(Body); | ||
3931 | |||
3932 | |||
3933 | // Non-Vehicles have a limited set of Hover options. | ||
3934 | // determine what our target height really is based on HoverType | ||
3935 | switch (m_PIDHoverType) | ||
3936 | { | ||
3937 | case PIDHoverType.Ground: | ||
3938 | m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); | ||
3939 | m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; | ||
3940 | break; | ||
3941 | case PIDHoverType.GroundAndWater: | ||
3942 | m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); | ||
3943 | m_waterHeight = _parent_scene.GetWaterLevel(); | ||
3944 | if (m_groundHeight > m_waterHeight) | ||
3945 | { | ||
3946 | m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; | ||
3947 | } | ||
3948 | else | ||
3949 | { | ||
3950 | m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; | ||
3951 | } | ||
3952 | break; | ||
3953 | |||
3954 | } // end switch (m_PIDHoverType) | ||
3955 | |||
3956 | |||
3957 | _target_velocity = | ||
3958 | new Vector3(0.0f, 0.0f, | ||
3959 | (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) | ||
3960 | ); | ||
3961 | |||
3962 | // if velocity is zero, use position control; otherwise, velocity control | ||
3963 | |||
3964 | if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) | ||
3965 | { | ||
3966 | // keep track of where we stopped. No more slippin' & slidin' | ||
3967 | |||
3968 | // We only want to deactivate the PID Controller if we think we want to have our surrogate | ||
3969 | // react to the physics scene by moving it's position. | ||
3970 | // Avatar to Avatar collisions | ||
3971 | // Prim to avatar collisions | ||
3972 | d.Vector3 dlinvel = vel; | ||
3973 | d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); | ||
3974 | d.BodySetLinearVel(Body, dlinvel.X, dlinvel.Y, dlinvel.Z); | ||
3975 | d.BodyAddForce(Body, 0, 0, fz); | ||
3976 | //KF this prevents furthur motions return; | ||
3977 | } | ||
3978 | else | ||
3979 | { | ||
3980 | _zeroFlag = false; | ||
3981 | |||
3982 | // We're flying and colliding with something | ||
3983 | fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); | ||
3984 | } | ||
3985 | } // end m_useHoverPID && !m_usePID | ||
3986 | |||
3987 | |||
3988 | /// Dynamics Apply Forces =================================================================================== | ||
3989 | fx *= m_mass; | ||
3990 | fy *= m_mass; | ||
3991 | //fz *= m_mass; | ||
3992 | fx += m_force.X; | ||
3993 | fy += m_force.Y; | ||
3994 | fz += m_force.Z; | ||
3995 | |||
3996 | //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); | ||
3997 | if (fx != 0 || fy != 0 || fz != 0) | ||
3998 | { | ||
3999 | //m_taintdisable = true; | ||
4000 | //base.RaiseOutOfBounds(Position); | ||
4001 | //d.BodySetLinearVel(Body, fx, fy, 0f); | ||
4002 | if (!d.BodyIsEnabled(Body)) | ||
4003 | { | ||
4004 | // A physical body at rest on a surface will auto-disable after a while, | ||
4005 | // this appears to re-enable it incase the surface it is upon vanishes, | ||
4006 | // and the body should fall again. | ||
4007 | d.BodySetLinearVel(Body, 0f, 0f, 0f); | ||
4008 | d.BodySetForce(Body, 0f, 0f, 0f); | ||
4009 | enableBodySoft(); | ||
4010 | } | ||
4011 | |||
4012 | // 35x10 = 350n times the mass per second applied maximum. | ||
4013 | float nmax = 35f * m_mass; | ||
4014 | float nmin = -35f * m_mass; | ||
4015 | |||
4016 | |||
4017 | if (fx > nmax) | ||
4018 | fx = nmax; | ||
4019 | if (fx < nmin) | ||
4020 | fx = nmin; | ||
4021 | if (fy > nmax) | ||
4022 | fy = nmax; | ||
4023 | if (fy < nmin) | ||
4024 | fy = nmin; | ||
4025 | d.BodyAddForce(Body, fx, fy, fz); | ||
4026 | } // end apply forces | ||
4027 | } // end Vehicle/Dynamics | ||
4028 | |||
4029 | /// RotLookAt / LookAt ================================================================================= | ||
4030 | if (m_useAPID) | ||
4031 | { | ||
4032 | // RotLookAt, apparently overrides all other rotation sources. Inputs: | ||
4033 | // Quaternion m_APIDTarget | ||
4034 | // float m_APIDStrength // From SL experiments, this is the time to get there | ||
4035 | // float m_APIDDamping // From SL experiments, this is damping, 1.0 = damped, 0.1 = wobbly | ||
4036 | // Also in SL the mass of the object has no effect on time to get there. | ||
4037 | // Factors: | ||
4038 | // get present body rotation | ||
4039 | float limit = 1.0f; | ||
4040 | float rscaler = 50f; // adjusts rotation damping time | ||
4041 | float lscaler = 10f; // adjusts linear damping time in llLookAt | ||
4042 | float RLAservo = 0f; | ||
4043 | Vector3 diff_axis; | ||
4044 | float diff_angle; | ||
4045 | d.Quaternion rot = d.BodyGetQuaternion(Body); // prim present rotation | ||
4046 | Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); | ||
4047 | Quaternion rtarget = new Quaternion(); | ||
4048 | |||
4049 | if (m_APIDTarget.W == -99.9f) | ||
4050 | { | ||
4051 | // this is really a llLookAt(), x,y,z is the target vector | ||
4052 | Vector3 target = new Vector3(m_APIDTarget.X, m_APIDTarget.Y, m_APIDTarget.Z); | ||
4053 | Vector3 ospin = new Vector3(1.0f, 0.0f, 0.0f) * rotq; | ||
4054 | Vector3 error = new Vector3(0.0f, 0.0f, 0.0f); | ||
4055 | float twopi = 2.0f * (float)Math.PI; | ||
4056 | Vector3 dir = target - _position; | ||
4057 | dir.Normalize(); | ||
4058 | float tzrot = (float)Math.Atan2(dir.Y, dir.X); | ||
4059 | float txy = (float)Math.Sqrt((dir.X * dir.X) + (dir.Y * dir.Y)); | ||
4060 | float terot = (float)Math.Atan2(dir.Z, txy); | ||
4061 | float ozrot = (float)Math.Atan2(ospin.Y, ospin.X); | ||
4062 | float oxy = (float)Math.Sqrt((ospin.X * ospin.X) + (ospin.Y * ospin.Y)); | ||
4063 | float oerot = (float)Math.Atan2(ospin.Z, oxy); | ||
4064 | float ra = 2.0f * ((rotq.W * rotq.X) + (rotq.Y * rotq.Z)); | ||
4065 | float rb = 1.0f - 2.0f * ((rotq.Y * rotq.Y) + (rotq.X * rotq.X)); | ||
4066 | float roll = (float)Math.Atan2(ra, rb); | ||
4067 | float errorz = tzrot - ozrot; | ||
4068 | if (errorz > (float)Math.PI) errorz -= twopi; | ||
4069 | else if (errorz < -(float)Math.PI) errorz += twopi; | ||
4070 | float errory = oerot - terot; | ||
4071 | if (errory > (float)Math.PI) errory -= twopi; | ||
4072 | else if (errory < -(float)Math.PI) errory += twopi; | ||
4073 | diff_angle = Math.Abs(errorz) + Math.Abs(errory) + Math.Abs(roll); | ||
4074 | if (diff_angle > 0.01f * m_APIDdamper) | ||
4075 | { | ||
4076 | m_APIDdamper = 1.0f; | ||
4077 | RLAservo = timestep / m_APIDStrength * rscaler; | ||
4078 | errorz *= RLAservo; | ||
4079 | errory *= RLAservo; | ||
4080 | error.X = -roll * 8.0f; | ||
4081 | error.Y = errory; | ||
4082 | error.Z = errorz; | ||
4083 | error *= rotq; | ||
4084 | d.BodySetAngularVel(Body, error.X, error.Y, error.Z); | ||
4085 | } | ||
4086 | else | ||
4087 | { | ||
4088 | d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f); | ||
4089 | m_APIDdamper = 2.0f; | ||
4090 | } | ||
4091 | } | ||
4092 | else | ||
4093 | { | ||
4094 | // this is a llRotLookAt() | ||
4095 | rtarget = m_APIDTarget; | ||
4096 | |||
4097 | Quaternion rot_diff = Quaternion.Inverse(rotq) * rtarget; // difference to desired rot | ||
4098 | rot_diff.GetAxisAngle(out diff_axis, out diff_angle); // convert to axis to point at & error angle | ||
4099 | //if(frcount == 0) Console.WriteLine("axis {0} angle {1}",diff_axis * 57.3f, diff_angle); | ||
4100 | |||
4101 | // diff_axis.Normalize(); it already is! | ||
4102 | if (diff_angle > 0.01f * m_APIDdamper) // diff_angle is always +ve // if there is enough error | ||
4103 | { | ||
4104 | m_APIDdamper = 1.0f; | ||
4105 | Vector3 rotforce = new Vector3(diff_axis.X, diff_axis.Y, diff_axis.Z); | ||
4106 | rotforce = rotforce * rotq; | ||
4107 | if (diff_angle > limit) diff_angle = limit; // cap the rotate rate | ||
4108 | RLAservo = timestep / m_APIDStrength * lscaler; | ||
4109 | rotforce = rotforce * RLAservo * diff_angle; | ||
4110 | d.BodySetAngularVel(Body, rotforce.X, rotforce.Y, rotforce.Z); | ||
4111 | //Console.WriteLine("axis= " + diff_axis + " angle= " + diff_angle + "servo= " + RLAservo); | ||
4112 | } | ||
4113 | else | ||
4114 | { // close enough | ||
4115 | d.BodySetAngularVel(Body, 0.0f, 0.0f, 0.0f); | ||
4116 | m_APIDdamper = 2.0f; | ||
4117 | } | ||
4118 | } // end llLookAt/llRotLookAt | ||
4119 | //if(frcount == 0) Console.WriteLine("mass= " + m_mass + " servo= " + RLAservo + " angle= " + diff_angle); | ||
4120 | } // end m_useAPID | ||
4121 | } // end root prims | ||
4122 | } // end Move() | ||
4123 | } // end class | ||
4124 | } | ||