diff options
author | UbitUmarov | 2015-09-08 15:03:22 +0100 |
---|---|---|
committer | UbitUmarov | 2015-09-08 15:03:22 +0100 |
commit | 5b3e2ab9aaf98892f18f733a3c5de816662be42a (patch) | |
tree | 03077685e44648264523e1851cd068d51a507e81 /OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs | |
parent | add script events per sec stat, using the time report code, but ignoring the... (diff) | |
parent | More 'everything is a module' merging. (diff) | |
download | opensim-SC-5b3e2ab9aaf98892f18f733a3c5de816662be42a.zip opensim-SC-5b3e2ab9aaf98892f18f733a3c5de816662be42a.tar.gz opensim-SC-5b3e2ab9aaf98892f18f733a3c5de816662be42a.tar.bz2 opensim-SC-5b3e2ab9aaf98892f18f733a3c5de816662be42a.tar.xz |
Merge branch 'mbworknew1' into ubitworkvarnew
Diffstat (limited to 'OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs')
-rw-r--r-- | OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs | 3435 |
1 files changed, 3435 insertions, 0 deletions
diff --git a/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs b/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs new file mode 100644 index 0000000..54a2033 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/Ode/ODEPrim.cs | |||
@@ -0,0 +1,3435 @@ | |||
1 | /* | ||
2 | * Copyright (c) Contributors, http://opensimulator.org/ | ||
3 | * See CONTRIBUTORS.TXT for a full list of copyright holders. | ||
4 | * | ||
5 | * Redistribution and use in source and binary forms, with or without | ||
6 | * modification, are permitted provided that the following conditions are met: | ||
7 | * * Redistributions of source code must retain the above copyright | ||
8 | * notice, this list of conditions and the following disclaimer. | ||
9 | * * Redistributions in binary form must reproduce the above copyright | ||
10 | * notice, this list of conditions and the following disclaimer in the | ||
11 | * documentation and/or other materials provided with the distribution. | ||
12 | * * Neither the name of the OpenSimulator Project nor the | ||
13 | * names of its contributors may be used to endorse or promote products | ||
14 | * derived from this software without specific prior written permission. | ||
15 | * | ||
16 | * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY | ||
17 | * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED | ||
18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE | ||
19 | * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY | ||
20 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES | ||
21 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
22 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND | ||
23 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
24 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS | ||
25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. | ||
26 | */ | ||
27 | |||
28 | /* | ||
29 | * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces | ||
30 | * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: | ||
31 | * ODEPrim.cs contains methods dealing with Prim editing, Prim | ||
32 | * characteristics and Kinetic motion. | ||
33 | * ODEDynamics.cs contains methods dealing with Prim Physical motion | ||
34 | * (dynamics) and the associated settings. Old Linear and angular | ||
35 | * motors for dynamic motion have been replace with MoveLinear() | ||
36 | * and MoveAngular(); 'Physical' is used only to switch ODE dynamic | ||
37 | * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to | ||
38 | * switch between 'VEHICLE' parameter use and general dynamics | ||
39 | * settings use. | ||
40 | */ | ||
41 | |||
42 | //#define SPAM | ||
43 | |||
44 | using System; | ||
45 | using System.Collections.Generic; | ||
46 | using System.Reflection; | ||
47 | using System.Runtime.InteropServices; | ||
48 | using System.Threading; | ||
49 | using log4net; | ||
50 | using OpenMetaverse; | ||
51 | using Ode.NET; | ||
52 | using OpenSim.Framework; | ||
53 | using OpenSim.Region.PhysicsModules.SharedBase; | ||
54 | |||
55 | namespace OpenSim.Region.PhysicsModule.ODE | ||
56 | { | ||
57 | /// <summary> | ||
58 | /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves. | ||
59 | /// </summary> | ||
60 | public class OdePrim : PhysicsActor | ||
61 | { | ||
62 | private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); | ||
63 | |||
64 | private bool m_isphysical; | ||
65 | |||
66 | public int ExpectedCollisionContacts { get { return m_expectedCollisionContacts; } } | ||
67 | private int m_expectedCollisionContacts = 0; | ||
68 | |||
69 | /// <summary> | ||
70 | /// Gets collide bits so that we can still perform land collisions if a mesh fails to load. | ||
71 | /// </summary> | ||
72 | private int BadMeshAssetCollideBits | ||
73 | { | ||
74 | get { return m_isphysical ? (int)CollisionCategories.Land : 0; } | ||
75 | } | ||
76 | |||
77 | /// <summary> | ||
78 | /// Is this prim subject to physics? Even if not, it's still solid for collision purposes. | ||
79 | /// </summary> | ||
80 | public override bool IsPhysical | ||
81 | { | ||
82 | get { return m_isphysical; } | ||
83 | set | ||
84 | { | ||
85 | m_isphysical = value; | ||
86 | if (!m_isphysical) // Zero the remembered last velocity | ||
87 | m_lastVelocity = Vector3.Zero; | ||
88 | } | ||
89 | } | ||
90 | |||
91 | private Vector3 _position; | ||
92 | private Vector3 _velocity; | ||
93 | private Vector3 _torque; | ||
94 | private Vector3 m_lastVelocity; | ||
95 | private Vector3 m_lastposition; | ||
96 | private Quaternion m_lastorientation = new Quaternion(); | ||
97 | private Vector3 m_rotationalVelocity; | ||
98 | private Vector3 _size; | ||
99 | private Vector3 _acceleration; | ||
100 | // private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f); | ||
101 | private Quaternion _orientation; | ||
102 | private Vector3 m_taintposition; | ||
103 | private Vector3 m_taintsize; | ||
104 | private Vector3 m_taintVelocity; | ||
105 | private Vector3 m_taintTorque; | ||
106 | private Quaternion m_taintrot; | ||
107 | private Vector3 m_angularlock = Vector3.One; | ||
108 | private Vector3 m_taintAngularLock = Vector3.One; | ||
109 | private IntPtr Amotor = IntPtr.Zero; | ||
110 | |||
111 | private bool m_assetFailed = false; | ||
112 | |||
113 | private Vector3 m_PIDTarget; | ||
114 | private float m_PIDTau; | ||
115 | private float PID_D = 35f; | ||
116 | private float PID_G = 25f; | ||
117 | |||
118 | // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), | ||
119 | // and are for non-VEHICLES only. | ||
120 | |||
121 | private float m_PIDHoverHeight; | ||
122 | private float m_PIDHoverTau; | ||
123 | private bool m_useHoverPID; | ||
124 | private PIDHoverType m_PIDHoverType = PIDHoverType.Ground; | ||
125 | private float m_targetHoverHeight; | ||
126 | private float m_groundHeight; | ||
127 | private float m_waterHeight; | ||
128 | private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. | ||
129 | |||
130 | // private float m_tensor = 5f; | ||
131 | private int body_autodisable_frames = 20; | ||
132 | |||
133 | |||
134 | private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom | ||
135 | | CollisionCategories.Space | ||
136 | | CollisionCategories.Body | ||
137 | | CollisionCategories.Character | ||
138 | ); | ||
139 | private bool m_taintshape; | ||
140 | private bool m_taintPhysics; | ||
141 | private bool m_collidesLand = true; | ||
142 | private bool m_collidesWater; | ||
143 | |||
144 | // Default we're a Geometry | ||
145 | private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); | ||
146 | |||
147 | // Default, Collide with Other Geometries, spaces and Bodies | ||
148 | private CollisionCategories m_collisionFlags = m_default_collisionFlags; | ||
149 | |||
150 | public bool m_taintremove { get; private set; } | ||
151 | public bool m_taintdisable { get; private set; } | ||
152 | internal bool m_disabled; | ||
153 | public bool m_taintadd { get; private set; } | ||
154 | public bool m_taintselected { get; private set; } | ||
155 | public bool m_taintCollidesWater { get; private set; } | ||
156 | |||
157 | private bool m_taintforce = false; | ||
158 | private bool m_taintaddangularforce = false; | ||
159 | private Vector3 m_force; | ||
160 | private List<Vector3> m_forcelist = new List<Vector3>(); | ||
161 | private List<Vector3> m_angularforcelist = new List<Vector3>(); | ||
162 | |||
163 | private PrimitiveBaseShape _pbs; | ||
164 | private OdeScene _parent_scene; | ||
165 | |||
166 | /// <summary> | ||
167 | /// The physics space which contains prim geometries | ||
168 | /// </summary> | ||
169 | public IntPtr m_targetSpace = IntPtr.Zero; | ||
170 | |||
171 | /// <summary> | ||
172 | /// The prim geometry, used for collision detection. | ||
173 | /// </summary> | ||
174 | /// <remarks> | ||
175 | /// This is never null except for a brief period when the geometry needs to be replaced (due to resizing or | ||
176 | /// mesh change) or when the physical prim is being removed from the scene. | ||
177 | /// </remarks> | ||
178 | public IntPtr prim_geom { get; private set; } | ||
179 | |||
180 | public IntPtr _triMeshData { get; private set; } | ||
181 | |||
182 | private IntPtr _linkJointGroup = IntPtr.Zero; | ||
183 | private PhysicsActor _parent; | ||
184 | private PhysicsActor m_taintparent; | ||
185 | |||
186 | private List<OdePrim> childrenPrim = new List<OdePrim>(); | ||
187 | |||
188 | private bool iscolliding; | ||
189 | private bool m_isSelected; | ||
190 | |||
191 | internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively | ||
192 | |||
193 | private bool m_throttleUpdates; | ||
194 | private int throttleCounter; | ||
195 | public int m_interpenetrationcount { get; private set; } | ||
196 | internal float m_collisionscore; | ||
197 | public int m_roundsUnderMotionThreshold { get; private set; } | ||
198 | private int m_crossingfailures; | ||
199 | |||
200 | public bool outofBounds { get; private set; } | ||
201 | private float m_density = 10.000006836f; // Aluminum g/cm3; | ||
202 | |||
203 | public bool _zeroFlag { get; private set; } | ||
204 | private bool m_lastUpdateSent; | ||
205 | |||
206 | public IntPtr Body = IntPtr.Zero; | ||
207 | private Vector3 _target_velocity; | ||
208 | private d.Mass pMass; | ||
209 | |||
210 | private int m_eventsubscription; | ||
211 | private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate(); | ||
212 | |||
213 | /// <summary> | ||
214 | /// Signal whether there were collisions on the previous frame, so we know if we need to send the | ||
215 | /// empty CollisionEventsThisFrame to the prim so that it can detect the end of a collision. | ||
216 | /// </summary> | ||
217 | /// <remarks> | ||
218 | /// This is probably a temporary measure, pending storing this information consistently in CollisionEventUpdate itself. | ||
219 | /// </remarks> | ||
220 | private bool m_collisionsOnPreviousFrame; | ||
221 | |||
222 | private IntPtr m_linkJoint = IntPtr.Zero; | ||
223 | |||
224 | internal volatile bool childPrim; | ||
225 | |||
226 | private ODEDynamics m_vehicle; | ||
227 | |||
228 | internal int m_material = (int)Material.Wood; | ||
229 | |||
230 | public OdePrim( | ||
231 | String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, | ||
232 | Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical) | ||
233 | { | ||
234 | Name = primName; | ||
235 | m_vehicle = new ODEDynamics(); | ||
236 | //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned); | ||
237 | |||
238 | if (!pos.IsFinite()) | ||
239 | { | ||
240 | pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), | ||
241 | parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); | ||
242 | m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name); | ||
243 | } | ||
244 | _position = pos; | ||
245 | m_taintposition = pos; | ||
246 | PID_D = parent_scene.bodyPIDD; | ||
247 | PID_G = parent_scene.bodyPIDG; | ||
248 | m_density = parent_scene.geomDefaultDensity; | ||
249 | // m_tensor = parent_scene.bodyMotorJointMaxforceTensor; | ||
250 | body_autodisable_frames = parent_scene.bodyFramesAutoDisable; | ||
251 | |||
252 | prim_geom = IntPtr.Zero; | ||
253 | |||
254 | if (!pos.IsFinite()) | ||
255 | { | ||
256 | size = new Vector3(0.5f, 0.5f, 0.5f); | ||
257 | m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name); | ||
258 | } | ||
259 | |||
260 | if (size.X <= 0) size.X = 0.01f; | ||
261 | if (size.Y <= 0) size.Y = 0.01f; | ||
262 | if (size.Z <= 0) size.Z = 0.01f; | ||
263 | |||
264 | _size = size; | ||
265 | m_taintsize = _size; | ||
266 | |||
267 | if (!QuaternionIsFinite(rotation)) | ||
268 | { | ||
269 | rotation = Quaternion.Identity; | ||
270 | m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name); | ||
271 | } | ||
272 | |||
273 | _orientation = rotation; | ||
274 | m_taintrot = _orientation; | ||
275 | _pbs = pbs; | ||
276 | |||
277 | _parent_scene = parent_scene; | ||
278 | m_targetSpace = (IntPtr)0; | ||
279 | |||
280 | if (pos.Z < 0) | ||
281 | { | ||
282 | IsPhysical = false; | ||
283 | } | ||
284 | else | ||
285 | { | ||
286 | IsPhysical = pisPhysical; | ||
287 | // If we're physical, we need to be in the master space for now. | ||
288 | // linksets *should* be in a space together.. but are not currently | ||
289 | if (IsPhysical) | ||
290 | m_targetSpace = _parent_scene.space; | ||
291 | } | ||
292 | |||
293 | m_taintadd = true; | ||
294 | m_assetFailed = false; | ||
295 | _parent_scene.AddPhysicsActorTaint(this); | ||
296 | } | ||
297 | |||
298 | public override int PhysicsActorType | ||
299 | { | ||
300 | get { return (int) ActorTypes.Prim; } | ||
301 | set { return; } | ||
302 | } | ||
303 | |||
304 | public override bool SetAlwaysRun | ||
305 | { | ||
306 | get { return false; } | ||
307 | set { return; } | ||
308 | } | ||
309 | |||
310 | public override bool Grabbed | ||
311 | { | ||
312 | set { return; } | ||
313 | } | ||
314 | |||
315 | public override bool Selected | ||
316 | { | ||
317 | set | ||
318 | { | ||
319 | // This only makes the object not collidable if the object | ||
320 | // is physical or the object is modified somehow *IN THE FUTURE* | ||
321 | // without this, if an avatar selects prim, they can walk right | ||
322 | // through it while it's selected | ||
323 | m_collisionscore = 0; | ||
324 | |||
325 | if ((IsPhysical && !_zeroFlag) || !value) | ||
326 | { | ||
327 | m_taintselected = value; | ||
328 | _parent_scene.AddPhysicsActorTaint(this); | ||
329 | } | ||
330 | else | ||
331 | { | ||
332 | m_taintselected = value; | ||
333 | m_isSelected = value; | ||
334 | } | ||
335 | |||
336 | if (m_isSelected) | ||
337 | disableBodySoft(); | ||
338 | } | ||
339 | } | ||
340 | |||
341 | /// <summary> | ||
342 | /// Set a new geometry for this prim. | ||
343 | /// </summary> | ||
344 | /// <param name="geom"></param> | ||
345 | private void SetGeom(IntPtr geom) | ||
346 | { | ||
347 | prim_geom = geom; | ||
348 | //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name); | ||
349 | |||
350 | if (m_assetFailed) | ||
351 | { | ||
352 | d.GeomSetCategoryBits(prim_geom, 0); | ||
353 | d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits); | ||
354 | } | ||
355 | else | ||
356 | { | ||
357 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | ||
358 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
359 | } | ||
360 | |||
361 | _parent_scene.geom_name_map[prim_geom] = Name; | ||
362 | _parent_scene.actor_name_map[prim_geom] = this; | ||
363 | |||
364 | if (childPrim) | ||
365 | { | ||
366 | if (_parent != null && _parent is OdePrim) | ||
367 | { | ||
368 | OdePrim parent = (OdePrim)_parent; | ||
369 | //Console.WriteLine("SetGeom calls ChildSetGeom"); | ||
370 | parent.ChildSetGeom(this); | ||
371 | } | ||
372 | } | ||
373 | //m_log.Warn("Setting Geom to: " + prim_geom); | ||
374 | } | ||
375 | |||
376 | private void enableBodySoft() | ||
377 | { | ||
378 | if (!childPrim) | ||
379 | { | ||
380 | if (IsPhysical && Body != IntPtr.Zero) | ||
381 | { | ||
382 | d.BodyEnable(Body); | ||
383 | if (m_vehicle.Type != Vehicle.TYPE_NONE) | ||
384 | m_vehicle.Enable(Body, _parent_scene); | ||
385 | } | ||
386 | |||
387 | m_disabled = false; | ||
388 | } | ||
389 | } | ||
390 | |||
391 | private void disableBodySoft() | ||
392 | { | ||
393 | m_disabled = true; | ||
394 | |||
395 | if (IsPhysical && Body != IntPtr.Zero) | ||
396 | { | ||
397 | d.BodyDisable(Body); | ||
398 | } | ||
399 | } | ||
400 | |||
401 | /// <summary> | ||
402 | /// Make a prim subject to physics. | ||
403 | /// </summary> | ||
404 | private void enableBody() | ||
405 | { | ||
406 | // Don't enable this body if we're a child prim | ||
407 | // this should be taken care of in the parent function not here | ||
408 | if (!childPrim) | ||
409 | { | ||
410 | // Sets the geom to a body | ||
411 | Body = d.BodyCreate(_parent_scene.world); | ||
412 | |||
413 | setMass(); | ||
414 | d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); | ||
415 | d.Quaternion myrot = new d.Quaternion(); | ||
416 | myrot.X = _orientation.X; | ||
417 | myrot.Y = _orientation.Y; | ||
418 | myrot.Z = _orientation.Z; | ||
419 | myrot.W = _orientation.W; | ||
420 | d.BodySetQuaternion(Body, ref myrot); | ||
421 | d.GeomSetBody(prim_geom, Body); | ||
422 | |||
423 | if (m_assetFailed) | ||
424 | { | ||
425 | d.GeomSetCategoryBits(prim_geom, 0); | ||
426 | d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits); | ||
427 | } | ||
428 | else | ||
429 | { | ||
430 | m_collisionCategories |= CollisionCategories.Body; | ||
431 | m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); | ||
432 | } | ||
433 | |||
434 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | ||
435 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
436 | |||
437 | d.BodySetAutoDisableFlag(Body, true); | ||
438 | d.BodySetAutoDisableSteps(Body, body_autodisable_frames); | ||
439 | |||
440 | // disconnect from world gravity so we can apply buoyancy | ||
441 | d.BodySetGravityMode (Body, false); | ||
442 | |||
443 | m_interpenetrationcount = 0; | ||
444 | m_collisionscore = 0; | ||
445 | m_disabled = false; | ||
446 | |||
447 | // The body doesn't already have a finite rotation mode set here | ||
448 | if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0.0f)) && _parent == null) | ||
449 | { | ||
450 | createAMotor(m_angularlock); | ||
451 | } | ||
452 | if (m_vehicle.Type != Vehicle.TYPE_NONE) | ||
453 | { | ||
454 | m_vehicle.Enable(Body, _parent_scene); | ||
455 | } | ||
456 | |||
457 | _parent_scene.ActivatePrim(this); | ||
458 | } | ||
459 | } | ||
460 | |||
461 | #region Mass Calculation | ||
462 | |||
463 | private float CalculateMass() | ||
464 | { | ||
465 | float volume = _size.X * _size.Y * _size.Z; // default | ||
466 | float tmp; | ||
467 | |||
468 | float returnMass = 0; | ||
469 | float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; | ||
470 | float hollowVolume = hollowAmount * hollowAmount; | ||
471 | |||
472 | switch (_pbs.ProfileShape) | ||
473 | { | ||
474 | case ProfileShape.Square: | ||
475 | // default box | ||
476 | |||
477 | if (_pbs.PathCurve == (byte)Extrusion.Straight) | ||
478 | { | ||
479 | if (hollowAmount > 0.0) | ||
480 | { | ||
481 | switch (_pbs.HollowShape) | ||
482 | { | ||
483 | case HollowShape.Square: | ||
484 | case HollowShape.Same: | ||
485 | break; | ||
486 | |||
487 | case HollowShape.Circle: | ||
488 | |||
489 | hollowVolume *= 0.78539816339f; | ||
490 | break; | ||
491 | |||
492 | case HollowShape.Triangle: | ||
493 | |||
494 | hollowVolume *= (0.5f * .5f); | ||
495 | break; | ||
496 | |||
497 | default: | ||
498 | hollowVolume = 0; | ||
499 | break; | ||
500 | } | ||
501 | volume *= (1.0f - hollowVolume); | ||
502 | } | ||
503 | } | ||
504 | |||
505 | else if (_pbs.PathCurve == (byte)Extrusion.Curve1) | ||
506 | { | ||
507 | //a tube | ||
508 | |||
509 | volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); | ||
510 | tmp= 1.0f -2.0e-2f * (float)(200 - _pbs.PathScaleY); | ||
511 | volume -= volume*tmp*tmp; | ||
512 | |||
513 | if (hollowAmount > 0.0) | ||
514 | { | ||
515 | hollowVolume *= hollowAmount; | ||
516 | |||
517 | switch (_pbs.HollowShape) | ||
518 | { | ||
519 | case HollowShape.Square: | ||
520 | case HollowShape.Same: | ||
521 | break; | ||
522 | |||
523 | case HollowShape.Circle: | ||
524 | hollowVolume *= 0.78539816339f;; | ||
525 | break; | ||
526 | |||
527 | case HollowShape.Triangle: | ||
528 | hollowVolume *= 0.5f * 0.5f; | ||
529 | break; | ||
530 | default: | ||
531 | hollowVolume = 0; | ||
532 | break; | ||
533 | } | ||
534 | volume *= (1.0f - hollowVolume); | ||
535 | } | ||
536 | } | ||
537 | |||
538 | break; | ||
539 | |||
540 | case ProfileShape.Circle: | ||
541 | |||
542 | if (_pbs.PathCurve == (byte)Extrusion.Straight) | ||
543 | { | ||
544 | volume *= 0.78539816339f; // elipse base | ||
545 | |||
546 | if (hollowAmount > 0.0) | ||
547 | { | ||
548 | switch (_pbs.HollowShape) | ||
549 | { | ||
550 | case HollowShape.Same: | ||
551 | case HollowShape.Circle: | ||
552 | break; | ||
553 | |||
554 | case HollowShape.Square: | ||
555 | hollowVolume *= 0.5f * 2.5984480504799f; | ||
556 | break; | ||
557 | |||
558 | case HollowShape.Triangle: | ||
559 | hollowVolume *= .5f * 1.27323954473516f; | ||
560 | break; | ||
561 | |||
562 | default: | ||
563 | hollowVolume = 0; | ||
564 | break; | ||
565 | } | ||
566 | volume *= (1.0f - hollowVolume); | ||
567 | } | ||
568 | } | ||
569 | |||
570 | else if (_pbs.PathCurve == (byte)Extrusion.Curve1) | ||
571 | { | ||
572 | volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); | ||
573 | tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); | ||
574 | volume *= (1.0f - tmp * tmp); | ||
575 | |||
576 | if (hollowAmount > 0.0) | ||
577 | { | ||
578 | |||
579 | // calculate the hollow volume by it's shape compared to the prim shape | ||
580 | hollowVolume *= hollowAmount; | ||
581 | |||
582 | switch (_pbs.HollowShape) | ||
583 | { | ||
584 | case HollowShape.Same: | ||
585 | case HollowShape.Circle: | ||
586 | break; | ||
587 | |||
588 | case HollowShape.Square: | ||
589 | hollowVolume *= 0.5f * 2.5984480504799f; | ||
590 | break; | ||
591 | |||
592 | case HollowShape.Triangle: | ||
593 | hollowVolume *= .5f * 1.27323954473516f; | ||
594 | break; | ||
595 | |||
596 | default: | ||
597 | hollowVolume = 0; | ||
598 | break; | ||
599 | } | ||
600 | volume *= (1.0f - hollowVolume); | ||
601 | } | ||
602 | } | ||
603 | break; | ||
604 | |||
605 | case ProfileShape.HalfCircle: | ||
606 | if (_pbs.PathCurve == (byte)Extrusion.Curve1) | ||
607 | { | ||
608 | volume *= 0.52359877559829887307710723054658f; | ||
609 | } | ||
610 | break; | ||
611 | |||
612 | case ProfileShape.EquilateralTriangle: | ||
613 | |||
614 | if (_pbs.PathCurve == (byte)Extrusion.Straight) | ||
615 | { | ||
616 | volume *= 0.32475953f; | ||
617 | |||
618 | if (hollowAmount > 0.0) | ||
619 | { | ||
620 | |||
621 | // calculate the hollow volume by it's shape compared to the prim shape | ||
622 | switch (_pbs.HollowShape) | ||
623 | { | ||
624 | case HollowShape.Same: | ||
625 | case HollowShape.Triangle: | ||
626 | hollowVolume *= .25f; | ||
627 | break; | ||
628 | |||
629 | case HollowShape.Square: | ||
630 | hollowVolume *= 0.499849f * 3.07920140172638f; | ||
631 | break; | ||
632 | |||
633 | case HollowShape.Circle: | ||
634 | // Hollow shape is a perfect cyllinder in respect to the cube's scale | ||
635 | // Cyllinder hollow volume calculation | ||
636 | |||
637 | hollowVolume *= 0.1963495f * 3.07920140172638f; | ||
638 | break; | ||
639 | |||
640 | default: | ||
641 | hollowVolume = 0; | ||
642 | break; | ||
643 | } | ||
644 | volume *= (1.0f - hollowVolume); | ||
645 | } | ||
646 | } | ||
647 | else if (_pbs.PathCurve == (byte)Extrusion.Curve1) | ||
648 | { | ||
649 | volume *= 0.32475953f; | ||
650 | volume *= 0.01f * (float)(200 - _pbs.PathScaleX); | ||
651 | tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); | ||
652 | volume *= (1.0f - tmp * tmp); | ||
653 | |||
654 | if (hollowAmount > 0.0) | ||
655 | { | ||
656 | |||
657 | hollowVolume *= hollowAmount; | ||
658 | |||
659 | switch (_pbs.HollowShape) | ||
660 | { | ||
661 | case HollowShape.Same: | ||
662 | case HollowShape.Triangle: | ||
663 | hollowVolume *= .25f; | ||
664 | break; | ||
665 | |||
666 | case HollowShape.Square: | ||
667 | hollowVolume *= 0.499849f * 3.07920140172638f; | ||
668 | break; | ||
669 | |||
670 | case HollowShape.Circle: | ||
671 | |||
672 | hollowVolume *= 0.1963495f * 3.07920140172638f; | ||
673 | break; | ||
674 | |||
675 | default: | ||
676 | hollowVolume = 0; | ||
677 | break; | ||
678 | } | ||
679 | volume *= (1.0f - hollowVolume); | ||
680 | } | ||
681 | } | ||
682 | break; | ||
683 | |||
684 | default: | ||
685 | break; | ||
686 | } | ||
687 | |||
688 | float taperX1; | ||
689 | float taperY1; | ||
690 | float taperX; | ||
691 | float taperY; | ||
692 | float pathBegin; | ||
693 | float pathEnd; | ||
694 | float profileBegin; | ||
695 | float profileEnd; | ||
696 | |||
697 | if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) | ||
698 | { | ||
699 | taperX1 = _pbs.PathScaleX * 0.01f; | ||
700 | if (taperX1 > 1.0f) | ||
701 | taperX1 = 2.0f - taperX1; | ||
702 | taperX = 1.0f - taperX1; | ||
703 | |||
704 | taperY1 = _pbs.PathScaleY * 0.01f; | ||
705 | if (taperY1 > 1.0f) | ||
706 | taperY1 = 2.0f - taperY1; | ||
707 | taperY = 1.0f - taperY1; | ||
708 | } | ||
709 | else | ||
710 | { | ||
711 | taperX = _pbs.PathTaperX * 0.01f; | ||
712 | if (taperX < 0.0f) | ||
713 | taperX = -taperX; | ||
714 | taperX1 = 1.0f - taperX; | ||
715 | |||
716 | taperY = _pbs.PathTaperY * 0.01f; | ||
717 | if (taperY < 0.0f) | ||
718 | taperY = -taperY; | ||
719 | taperY1 = 1.0f - taperY; | ||
720 | } | ||
721 | |||
722 | volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); | ||
723 | |||
724 | pathBegin = (float)_pbs.PathBegin * 2.0e-5f; | ||
725 | pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; | ||
726 | volume *= (pathEnd - pathBegin); | ||
727 | |||
728 | // this is crude aproximation | ||
729 | profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; | ||
730 | profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; | ||
731 | volume *= (profileEnd - profileBegin); | ||
732 | |||
733 | returnMass = m_density * volume; | ||
734 | |||
735 | if (returnMass <= 0) | ||
736 | returnMass = 0.0001f;//ckrinke: Mass must be greater then zero. | ||
737 | // else if (returnMass > _parent_scene.maximumMassObject) | ||
738 | // returnMass = _parent_scene.maximumMassObject; | ||
739 | |||
740 | // Recursively calculate mass | ||
741 | bool HasChildPrim = false; | ||
742 | lock (childrenPrim) | ||
743 | { | ||
744 | if (childrenPrim.Count > 0) | ||
745 | { | ||
746 | HasChildPrim = true; | ||
747 | } | ||
748 | } | ||
749 | |||
750 | if (HasChildPrim) | ||
751 | { | ||
752 | OdePrim[] childPrimArr = new OdePrim[0]; | ||
753 | |||
754 | lock (childrenPrim) | ||
755 | childPrimArr = childrenPrim.ToArray(); | ||
756 | |||
757 | for (int i = 0; i < childPrimArr.Length; i++) | ||
758 | { | ||
759 | if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove) | ||
760 | returnMass += childPrimArr[i].CalculateMass(); | ||
761 | // failsafe, this shouldn't happen but with OpenSim, you never know :) | ||
762 | if (i > 256) | ||
763 | break; | ||
764 | } | ||
765 | } | ||
766 | |||
767 | if (returnMass > _parent_scene.maximumMassObject) | ||
768 | returnMass = _parent_scene.maximumMassObject; | ||
769 | |||
770 | return returnMass; | ||
771 | } | ||
772 | |||
773 | #endregion | ||
774 | |||
775 | private void setMass() | ||
776 | { | ||
777 | if (Body != (IntPtr) 0) | ||
778 | { | ||
779 | float newmass = CalculateMass(); | ||
780 | |||
781 | //m_log.Info("[PHYSICS]: New Mass: " + newmass.ToString()); | ||
782 | |||
783 | d.MassSetBoxTotal(out pMass, newmass, _size.X, _size.Y, _size.Z); | ||
784 | d.BodySetMass(Body, ref pMass); | ||
785 | } | ||
786 | } | ||
787 | |||
788 | private void setAngularVelocity(float x, float y, float z) | ||
789 | { | ||
790 | if (Body != (IntPtr)0) | ||
791 | { | ||
792 | d.BodySetAngularVel(Body, x, y, z); | ||
793 | } | ||
794 | } | ||
795 | |||
796 | /// <summary> | ||
797 | /// Stop a prim from being subject to physics. | ||
798 | /// </summary> | ||
799 | internal void disableBody() | ||
800 | { | ||
801 | //this kills the body so things like 'mesh' can re-create it. | ||
802 | lock (this) | ||
803 | { | ||
804 | if (!childPrim) | ||
805 | { | ||
806 | if (Body != IntPtr.Zero) | ||
807 | { | ||
808 | _parent_scene.DeactivatePrim(this); | ||
809 | m_collisionCategories &= ~CollisionCategories.Body; | ||
810 | m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); | ||
811 | |||
812 | if (m_assetFailed) | ||
813 | { | ||
814 | d.GeomSetCategoryBits(prim_geom, 0); | ||
815 | d.GeomSetCollideBits(prim_geom, 0); | ||
816 | } | ||
817 | else | ||
818 | { | ||
819 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | ||
820 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
821 | } | ||
822 | |||
823 | d.BodyDestroy(Body); | ||
824 | lock (childrenPrim) | ||
825 | { | ||
826 | if (childrenPrim.Count > 0) | ||
827 | { | ||
828 | foreach (OdePrim prm in childrenPrim) | ||
829 | { | ||
830 | _parent_scene.DeactivatePrim(prm); | ||
831 | prm.Body = IntPtr.Zero; | ||
832 | } | ||
833 | } | ||
834 | } | ||
835 | Body = IntPtr.Zero; | ||
836 | } | ||
837 | } | ||
838 | else | ||
839 | { | ||
840 | _parent_scene.DeactivatePrim(this); | ||
841 | |||
842 | m_collisionCategories &= ~CollisionCategories.Body; | ||
843 | m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); | ||
844 | |||
845 | if (m_assetFailed) | ||
846 | { | ||
847 | d.GeomSetCategoryBits(prim_geom, 0); | ||
848 | d.GeomSetCollideBits(prim_geom, 0); | ||
849 | } | ||
850 | else | ||
851 | { | ||
852 | |||
853 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | ||
854 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
855 | } | ||
856 | |||
857 | Body = IntPtr.Zero; | ||
858 | } | ||
859 | } | ||
860 | |||
861 | m_disabled = true; | ||
862 | m_collisionscore = 0; | ||
863 | } | ||
864 | |||
865 | private static Dictionary<IMesh, IntPtr> m_MeshToTriMeshMap = new Dictionary<IMesh, IntPtr>(); | ||
866 | |||
867 | private void setMesh(OdeScene parent_scene, IMesh mesh) | ||
868 | { | ||
869 | // m_log.DebugFormat("[ODE PRIM]: Setting mesh on {0} to {1}", Name, mesh); | ||
870 | |||
871 | // This sleeper is there to moderate how long it takes between | ||
872 | // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object | ||
873 | |||
874 | //Thread.Sleep(10); | ||
875 | |||
876 | //Kill Body so that mesh can re-make the geom | ||
877 | if (IsPhysical && Body != IntPtr.Zero) | ||
878 | { | ||
879 | if (childPrim) | ||
880 | { | ||
881 | if (_parent != null) | ||
882 | { | ||
883 | OdePrim parent = (OdePrim)_parent; | ||
884 | parent.ChildDelink(this); | ||
885 | } | ||
886 | } | ||
887 | else | ||
888 | { | ||
889 | disableBody(); | ||
890 | } | ||
891 | } | ||
892 | |||
893 | IntPtr vertices, indices; | ||
894 | int vertexCount, indexCount; | ||
895 | int vertexStride, triStride; | ||
896 | mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap | ||
897 | mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage | ||
898 | m_expectedCollisionContacts = indexCount; | ||
899 | mesh.releaseSourceMeshData(); // free up the original mesh data to save memory | ||
900 | |||
901 | // We must lock here since m_MeshToTriMeshMap is static and multiple scene threads may call this method at | ||
902 | // the same time. | ||
903 | lock (m_MeshToTriMeshMap) | ||
904 | { | ||
905 | if (m_MeshToTriMeshMap.ContainsKey(mesh)) | ||
906 | { | ||
907 | _triMeshData = m_MeshToTriMeshMap[mesh]; | ||
908 | } | ||
909 | else | ||
910 | { | ||
911 | _triMeshData = d.GeomTriMeshDataCreate(); | ||
912 | |||
913 | d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); | ||
914 | d.GeomTriMeshDataPreprocess(_triMeshData); | ||
915 | m_MeshToTriMeshMap[mesh] = _triMeshData; | ||
916 | } | ||
917 | } | ||
918 | |||
919 | // _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
920 | try | ||
921 | { | ||
922 | SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null)); | ||
923 | } | ||
924 | catch (AccessViolationException) | ||
925 | { | ||
926 | m_log.ErrorFormat("[PHYSICS]: MESH LOCKED FOR {0}", Name); | ||
927 | return; | ||
928 | } | ||
929 | |||
930 | // if (IsPhysical && Body == (IntPtr) 0) | ||
931 | // { | ||
932 | // Recreate the body | ||
933 | // m_interpenetrationcount = 0; | ||
934 | // m_collisionscore = 0; | ||
935 | |||
936 | // enableBody(); | ||
937 | // } | ||
938 | } | ||
939 | |||
940 | internal void ProcessTaints() | ||
941 | { | ||
942 | #if SPAM | ||
943 | Console.WriteLine("ZProcessTaints for " + Name); | ||
944 | #endif | ||
945 | |||
946 | // This must be processed as the very first taint so that later operations have a prim_geom to work with | ||
947 | // if this is a new prim. | ||
948 | if (m_taintadd) | ||
949 | changeadd(); | ||
950 | |||
951 | if (!_position.ApproxEquals(m_taintposition, 0f)) | ||
952 | changemove(); | ||
953 | |||
954 | if (m_taintrot != _orientation) | ||
955 | { | ||
956 | if (childPrim && IsPhysical) // For physical child prim... | ||
957 | { | ||
958 | rotate(); | ||
959 | // KF: ODE will also rotate the parent prim! | ||
960 | // so rotate the root back to where it was | ||
961 | OdePrim parent = (OdePrim)_parent; | ||
962 | parent.rotate(); | ||
963 | } | ||
964 | else | ||
965 | { | ||
966 | //Just rotate the prim | ||
967 | rotate(); | ||
968 | } | ||
969 | } | ||
970 | |||
971 | if (m_taintPhysics != IsPhysical && !(m_taintparent != _parent)) | ||
972 | changePhysicsStatus(); | ||
973 | |||
974 | if (!_size.ApproxEquals(m_taintsize, 0f)) | ||
975 | changesize(); | ||
976 | |||
977 | if (m_taintshape) | ||
978 | changeshape(); | ||
979 | |||
980 | if (m_taintforce) | ||
981 | changeAddForce(); | ||
982 | |||
983 | if (m_taintaddangularforce) | ||
984 | changeAddAngularForce(); | ||
985 | |||
986 | if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f)) | ||
987 | changeSetTorque(); | ||
988 | |||
989 | if (m_taintdisable) | ||
990 | changedisable(); | ||
991 | |||
992 | if (m_taintselected != m_isSelected) | ||
993 | changeSelectedStatus(); | ||
994 | |||
995 | if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f)) | ||
996 | changevelocity(); | ||
997 | |||
998 | if (m_taintparent != _parent) | ||
999 | changelink(); | ||
1000 | |||
1001 | if (m_taintCollidesWater != m_collidesWater) | ||
1002 | changefloatonwater(); | ||
1003 | |||
1004 | if (!m_angularlock.ApproxEquals(m_taintAngularLock,0f)) | ||
1005 | changeAngularLock(); | ||
1006 | } | ||
1007 | |||
1008 | /// <summary> | ||
1009 | /// Change prim in response to an angular lock taint. | ||
1010 | /// </summary> | ||
1011 | private void changeAngularLock() | ||
1012 | { | ||
1013 | // do we have a Physical object? | ||
1014 | if (Body != IntPtr.Zero) | ||
1015 | { | ||
1016 | //Check that we have a Parent | ||
1017 | //If we have a parent then we're not authorative here | ||
1018 | if (_parent == null) | ||
1019 | { | ||
1020 | if (!m_taintAngularLock.ApproxEquals(Vector3.One, 0f)) | ||
1021 | { | ||
1022 | //d.BodySetFiniteRotationMode(Body, 0); | ||
1023 | //d.BodySetFiniteRotationAxis(Body,m_taintAngularLock.X,m_taintAngularLock.Y,m_taintAngularLock.Z); | ||
1024 | createAMotor(m_taintAngularLock); | ||
1025 | } | ||
1026 | else | ||
1027 | { | ||
1028 | if (Amotor != IntPtr.Zero) | ||
1029 | { | ||
1030 | d.JointDestroy(Amotor); | ||
1031 | Amotor = IntPtr.Zero; | ||
1032 | } | ||
1033 | } | ||
1034 | } | ||
1035 | } | ||
1036 | |||
1037 | // Store this for later in case we get turned into a separate body | ||
1038 | m_angularlock = m_taintAngularLock; | ||
1039 | } | ||
1040 | |||
1041 | /// <summary> | ||
1042 | /// Change prim in response to a link taint. | ||
1043 | /// </summary> | ||
1044 | private void changelink() | ||
1045 | { | ||
1046 | // If the newly set parent is not null | ||
1047 | // create link | ||
1048 | if (_parent == null && m_taintparent != null) | ||
1049 | { | ||
1050 | if (m_taintparent.PhysicsActorType == (int)ActorTypes.Prim) | ||
1051 | { | ||
1052 | OdePrim obj = (OdePrim)m_taintparent; | ||
1053 | //obj.disableBody(); | ||
1054 | //Console.WriteLine("changelink calls ParentPrim"); | ||
1055 | obj.AddChildPrim(this); | ||
1056 | |||
1057 | /* | ||
1058 | if (obj.Body != (IntPtr)0 && Body != (IntPtr)0 && obj.Body != Body) | ||
1059 | { | ||
1060 | _linkJointGroup = d.JointGroupCreate(0); | ||
1061 | m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); | ||
1062 | d.JointAttach(m_linkJoint, obj.Body, Body); | ||
1063 | d.JointSetFixed(m_linkJoint); | ||
1064 | } | ||
1065 | */ | ||
1066 | } | ||
1067 | } | ||
1068 | // If the newly set parent is null | ||
1069 | // destroy link | ||
1070 | else if (_parent != null && m_taintparent == null) | ||
1071 | { | ||
1072 | //Console.WriteLine(" changelink B"); | ||
1073 | |||
1074 | if (_parent is OdePrim) | ||
1075 | { | ||
1076 | OdePrim obj = (OdePrim)_parent; | ||
1077 | obj.ChildDelink(this); | ||
1078 | childPrim = false; | ||
1079 | //_parent = null; | ||
1080 | } | ||
1081 | |||
1082 | /* | ||
1083 | if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0) | ||
1084 | d.JointGroupDestroy(_linkJointGroup); | ||
1085 | |||
1086 | _linkJointGroup = (IntPtr)0; | ||
1087 | m_linkJoint = (IntPtr)0; | ||
1088 | */ | ||
1089 | } | ||
1090 | |||
1091 | _parent = m_taintparent; | ||
1092 | m_taintPhysics = IsPhysical; | ||
1093 | } | ||
1094 | |||
1095 | /// <summary> | ||
1096 | /// Add a child prim to this parent prim. | ||
1097 | /// </summary> | ||
1098 | /// <param name="prim">Child prim</param> | ||
1099 | private void AddChildPrim(OdePrim prim) | ||
1100 | { | ||
1101 | if (LocalID == prim.LocalID) | ||
1102 | return; | ||
1103 | |||
1104 | if (Body == IntPtr.Zero) | ||
1105 | { | ||
1106 | Body = d.BodyCreate(_parent_scene.world); | ||
1107 | setMass(); | ||
1108 | } | ||
1109 | |||
1110 | lock (childrenPrim) | ||
1111 | { | ||
1112 | if (childrenPrim.Contains(prim)) | ||
1113 | return; | ||
1114 | |||
1115 | // m_log.DebugFormat( | ||
1116 | // "[ODE PRIM]: Linking prim {0} {1} to {2} {3}", prim.Name, prim.LocalID, Name, LocalID); | ||
1117 | |||
1118 | childrenPrim.Add(prim); | ||
1119 | |||
1120 | foreach (OdePrim prm in childrenPrim) | ||
1121 | { | ||
1122 | d.Mass m2; | ||
1123 | d.MassSetZero(out m2); | ||
1124 | d.MassSetBoxTotal(out m2, prim.CalculateMass(), prm._size.X, prm._size.Y, prm._size.Z); | ||
1125 | |||
1126 | d.Quaternion quat = new d.Quaternion(); | ||
1127 | quat.W = prm._orientation.W; | ||
1128 | quat.X = prm._orientation.X; | ||
1129 | quat.Y = prm._orientation.Y; | ||
1130 | quat.Z = prm._orientation.Z; | ||
1131 | |||
1132 | d.Matrix3 mat = new d.Matrix3(); | ||
1133 | d.RfromQ(out mat, ref quat); | ||
1134 | d.MassRotate(ref m2, ref mat); | ||
1135 | d.MassTranslate(ref m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z); | ||
1136 | d.MassAdd(ref pMass, ref m2); | ||
1137 | } | ||
1138 | |||
1139 | foreach (OdePrim prm in childrenPrim) | ||
1140 | { | ||
1141 | prm.m_collisionCategories |= CollisionCategories.Body; | ||
1142 | prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); | ||
1143 | |||
1144 | //Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + Name); | ||
1145 | if (prm.m_assetFailed) | ||
1146 | { | ||
1147 | d.GeomSetCategoryBits(prm.prim_geom, 0); | ||
1148 | d.GeomSetCollideBits(prm.prim_geom, prm.BadMeshAssetCollideBits); | ||
1149 | } | ||
1150 | else | ||
1151 | { | ||
1152 | d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); | ||
1153 | d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); | ||
1154 | } | ||
1155 | |||
1156 | d.Quaternion quat = new d.Quaternion(); | ||
1157 | quat.W = prm._orientation.W; | ||
1158 | quat.X = prm._orientation.X; | ||
1159 | quat.Y = prm._orientation.Y; | ||
1160 | quat.Z = prm._orientation.Z; | ||
1161 | |||
1162 | d.Matrix3 mat = new d.Matrix3(); | ||
1163 | d.RfromQ(out mat, ref quat); | ||
1164 | if (Body != IntPtr.Zero) | ||
1165 | { | ||
1166 | d.GeomSetBody(prm.prim_geom, Body); | ||
1167 | prm.childPrim = true; | ||
1168 | d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z); | ||
1169 | //d.GeomSetOffsetPosition(prim.prim_geom, | ||
1170 | // (Position.X - prm.Position.X) - pMass.c.X, | ||
1171 | // (Position.Y - prm.Position.Y) - pMass.c.Y, | ||
1172 | // (Position.Z - prm.Position.Z) - pMass.c.Z); | ||
1173 | d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); | ||
1174 | //d.GeomSetOffsetRotation(prm.prim_geom, ref mat); | ||
1175 | d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z); | ||
1176 | d.BodySetMass(Body, ref pMass); | ||
1177 | } | ||
1178 | else | ||
1179 | { | ||
1180 | m_log.DebugFormat("[PHYSICS]: {0} ain't got no boooooooooddy, no body", Name); | ||
1181 | } | ||
1182 | |||
1183 | prm.m_interpenetrationcount = 0; | ||
1184 | prm.m_collisionscore = 0; | ||
1185 | prm.m_disabled = false; | ||
1186 | |||
1187 | // The body doesn't already have a finite rotation mode set here | ||
1188 | if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null) | ||
1189 | { | ||
1190 | prm.createAMotor(m_angularlock); | ||
1191 | } | ||
1192 | prm.Body = Body; | ||
1193 | _parent_scene.ActivatePrim(prm); | ||
1194 | } | ||
1195 | |||
1196 | m_collisionCategories |= CollisionCategories.Body; | ||
1197 | m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); | ||
1198 | |||
1199 | if (m_assetFailed) | ||
1200 | { | ||
1201 | d.GeomSetCategoryBits(prim_geom, 0); | ||
1202 | d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits); | ||
1203 | } | ||
1204 | else | ||
1205 | { | ||
1206 | //Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + Name); | ||
1207 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | ||
1208 | //Console.WriteLine(" Post GeomSetCategoryBits 2"); | ||
1209 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
1210 | } | ||
1211 | |||
1212 | d.Quaternion quat2 = new d.Quaternion(); | ||
1213 | quat2.W = _orientation.W; | ||
1214 | quat2.X = _orientation.X; | ||
1215 | quat2.Y = _orientation.Y; | ||
1216 | quat2.Z = _orientation.Z; | ||
1217 | |||
1218 | d.Matrix3 mat2 = new d.Matrix3(); | ||
1219 | d.RfromQ(out mat2, ref quat2); | ||
1220 | d.GeomSetBody(prim_geom, Body); | ||
1221 | d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z); | ||
1222 | //d.GeomSetOffsetPosition(prim.prim_geom, | ||
1223 | // (Position.X - prm.Position.X) - pMass.c.X, | ||
1224 | // (Position.Y - prm.Position.Y) - pMass.c.Y, | ||
1225 | // (Position.Z - prm.Position.Z) - pMass.c.Z); | ||
1226 | //d.GeomSetOffsetRotation(prim_geom, ref mat2); | ||
1227 | d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z); | ||
1228 | d.BodySetMass(Body, ref pMass); | ||
1229 | |||
1230 | d.BodySetAutoDisableFlag(Body, true); | ||
1231 | d.BodySetAutoDisableSteps(Body, body_autodisable_frames); | ||
1232 | |||
1233 | m_interpenetrationcount = 0; | ||
1234 | m_collisionscore = 0; | ||
1235 | m_disabled = false; | ||
1236 | |||
1237 | // The body doesn't already have a finite rotation mode set here | ||
1238 | if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null) | ||
1239 | { | ||
1240 | createAMotor(m_angularlock); | ||
1241 | } | ||
1242 | |||
1243 | d.BodySetPosition(Body, Position.X, Position.Y, Position.Z); | ||
1244 | |||
1245 | if (m_vehicle.Type != Vehicle.TYPE_NONE) | ||
1246 | m_vehicle.Enable(Body, _parent_scene); | ||
1247 | |||
1248 | _parent_scene.ActivatePrim(this); | ||
1249 | } | ||
1250 | } | ||
1251 | |||
1252 | private void ChildSetGeom(OdePrim odePrim) | ||
1253 | { | ||
1254 | // m_log.DebugFormat( | ||
1255 | // "[ODE PRIM]: ChildSetGeom {0} {1} for {2} {3}", odePrim.Name, odePrim.LocalID, Name, LocalID); | ||
1256 | |||
1257 | //if (IsPhysical && Body != IntPtr.Zero) | ||
1258 | lock (childrenPrim) | ||
1259 | { | ||
1260 | foreach (OdePrim prm in childrenPrim) | ||
1261 | { | ||
1262 | //prm.childPrim = true; | ||
1263 | prm.disableBody(); | ||
1264 | //prm.m_taintparent = null; | ||
1265 | //prm._parent = null; | ||
1266 | //prm.m_taintPhysics = false; | ||
1267 | //prm.m_disabled = true; | ||
1268 | //prm.childPrim = false; | ||
1269 | } | ||
1270 | } | ||
1271 | |||
1272 | disableBody(); | ||
1273 | |||
1274 | // Spurious - Body == IntPtr.Zero after disableBody() | ||
1275 | // if (Body != IntPtr.Zero) | ||
1276 | // { | ||
1277 | // _parent_scene.DeactivatePrim(this); | ||
1278 | // } | ||
1279 | |||
1280 | lock (childrenPrim) | ||
1281 | { | ||
1282 | foreach (OdePrim prm in childrenPrim) | ||
1283 | { | ||
1284 | //Console.WriteLine("ChildSetGeom calls ParentPrim"); | ||
1285 | AddChildPrim(prm); | ||
1286 | } | ||
1287 | } | ||
1288 | } | ||
1289 | |||
1290 | private void ChildDelink(OdePrim odePrim) | ||
1291 | { | ||
1292 | // m_log.DebugFormat( | ||
1293 | // "[ODE PRIM]: Delinking prim {0} {1} from {2} {3}", odePrim.Name, odePrim.LocalID, Name, LocalID); | ||
1294 | |||
1295 | // Okay, we have a delinked child.. need to rebuild the body. | ||
1296 | lock (childrenPrim) | ||
1297 | { | ||
1298 | foreach (OdePrim prm in childrenPrim) | ||
1299 | { | ||
1300 | prm.childPrim = true; | ||
1301 | prm.disableBody(); | ||
1302 | //prm.m_taintparent = null; | ||
1303 | //prm._parent = null; | ||
1304 | //prm.m_taintPhysics = false; | ||
1305 | //prm.m_disabled = true; | ||
1306 | //prm.childPrim = false; | ||
1307 | } | ||
1308 | } | ||
1309 | |||
1310 | disableBody(); | ||
1311 | |||
1312 | lock (childrenPrim) | ||
1313 | { | ||
1314 | //Console.WriteLine("childrenPrim.Remove " + odePrim); | ||
1315 | childrenPrim.Remove(odePrim); | ||
1316 | } | ||
1317 | |||
1318 | // Spurious - Body == IntPtr.Zero after disableBody() | ||
1319 | // if (Body != IntPtr.Zero) | ||
1320 | // { | ||
1321 | // _parent_scene.DeactivatePrim(this); | ||
1322 | // } | ||
1323 | |||
1324 | lock (childrenPrim) | ||
1325 | { | ||
1326 | foreach (OdePrim prm in childrenPrim) | ||
1327 | { | ||
1328 | //Console.WriteLine("ChildDelink calls ParentPrim"); | ||
1329 | AddChildPrim(prm); | ||
1330 | } | ||
1331 | } | ||
1332 | } | ||
1333 | |||
1334 | /// <summary> | ||
1335 | /// Change prim in response to a selection taint. | ||
1336 | /// </summary> | ||
1337 | private void changeSelectedStatus() | ||
1338 | { | ||
1339 | if (m_taintselected) | ||
1340 | { | ||
1341 | m_collisionCategories = CollisionCategories.Selected; | ||
1342 | m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); | ||
1343 | |||
1344 | // We do the body disable soft twice because 'in theory' a collision could have happened | ||
1345 | // in between the disabling and the collision properties setting | ||
1346 | // which would wake the physical body up from a soft disabling and potentially cause it to fall | ||
1347 | // through the ground. | ||
1348 | |||
1349 | // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select | ||
1350 | // just one part of the assembly, the rest of the assembly is non-selected and still simulating, | ||
1351 | // so that causes the selected part to wake up and continue moving. | ||
1352 | |||
1353 | // even if you select all parts of a jointed assembly, it is not guaranteed that the entire | ||
1354 | // assembly will stop simulating during the selection, because of the lack of atomicity | ||
1355 | // of select operations (their processing could be interrupted by a thread switch, causing | ||
1356 | // simulation to continue before all of the selected object notifications trickle down to | ||
1357 | // the physics engine). | ||
1358 | |||
1359 | // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are | ||
1360 | // selected and disabled. then, due to a thread switch, the selection processing is | ||
1361 | // interrupted and the physics engine continues to simulate, so the last 50 items, whose | ||
1362 | // selection was not yet processed, continues to simulate. this wakes up ALL of the | ||
1363 | // first 50 again. then the last 50 are disabled. then the first 50, which were just woken | ||
1364 | // up, start simulating again, which in turn wakes up the last 50. | ||
1365 | |||
1366 | if (IsPhysical) | ||
1367 | { | ||
1368 | disableBodySoft(); | ||
1369 | } | ||
1370 | |||
1371 | if (m_assetFailed) | ||
1372 | { | ||
1373 | d.GeomSetCategoryBits(prim_geom, 0); | ||
1374 | d.GeomSetCollideBits(prim_geom, 0); | ||
1375 | } | ||
1376 | else | ||
1377 | { | ||
1378 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | ||
1379 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
1380 | } | ||
1381 | |||
1382 | if (IsPhysical) | ||
1383 | { | ||
1384 | disableBodySoft(); | ||
1385 | } | ||
1386 | } | ||
1387 | else | ||
1388 | { | ||
1389 | m_collisionCategories = CollisionCategories.Geom; | ||
1390 | |||
1391 | if (IsPhysical) | ||
1392 | m_collisionCategories |= CollisionCategories.Body; | ||
1393 | |||
1394 | m_collisionFlags = m_default_collisionFlags; | ||
1395 | |||
1396 | if (m_collidesLand) | ||
1397 | m_collisionFlags |= CollisionCategories.Land; | ||
1398 | if (m_collidesWater) | ||
1399 | m_collisionFlags |= CollisionCategories.Water; | ||
1400 | |||
1401 | if (m_assetFailed) | ||
1402 | { | ||
1403 | d.GeomSetCategoryBits(prim_geom, 0); | ||
1404 | d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits); | ||
1405 | } | ||
1406 | else | ||
1407 | { | ||
1408 | d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); | ||
1409 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
1410 | } | ||
1411 | |||
1412 | if (IsPhysical) | ||
1413 | { | ||
1414 | if (Body != IntPtr.Zero) | ||
1415 | { | ||
1416 | d.BodySetLinearVel(Body, 0f, 0f, 0f); | ||
1417 | d.BodySetForce(Body, 0, 0, 0); | ||
1418 | enableBodySoft(); | ||
1419 | } | ||
1420 | } | ||
1421 | } | ||
1422 | |||
1423 | resetCollisionAccounting(); | ||
1424 | m_isSelected = m_taintselected; | ||
1425 | }//end changeSelectedStatus | ||
1426 | |||
1427 | internal void ResetTaints() | ||
1428 | { | ||
1429 | m_taintposition = _position; | ||
1430 | m_taintrot = _orientation; | ||
1431 | m_taintPhysics = IsPhysical; | ||
1432 | m_taintselected = m_isSelected; | ||
1433 | m_taintsize = _size; | ||
1434 | m_taintshape = false; | ||
1435 | m_taintforce = false; | ||
1436 | m_taintdisable = false; | ||
1437 | m_taintVelocity = Vector3.Zero; | ||
1438 | } | ||
1439 | |||
1440 | /// <summary> | ||
1441 | /// Create a geometry for the given mesh in the given target space. | ||
1442 | /// </summary> | ||
1443 | /// <param name="m_targetSpace"></param> | ||
1444 | /// <param name="mesh">If null, then a mesh is used that is based on the profile shape data.</param> | ||
1445 | private void CreateGeom(IntPtr m_targetSpace, IMesh mesh) | ||
1446 | { | ||
1447 | #if SPAM | ||
1448 | Console.WriteLine("CreateGeom:"); | ||
1449 | #endif | ||
1450 | if (mesh != null) | ||
1451 | { | ||
1452 | setMesh(_parent_scene, mesh); | ||
1453 | } | ||
1454 | else | ||
1455 | { | ||
1456 | if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1) | ||
1457 | { | ||
1458 | if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z) | ||
1459 | { | ||
1460 | if (((_size.X / 2f) > 0f)) | ||
1461 | { | ||
1462 | // _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
1463 | try | ||
1464 | { | ||
1465 | //Console.WriteLine(" CreateGeom 1"); | ||
1466 | SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2)); | ||
1467 | m_expectedCollisionContacts = 3; | ||
1468 | } | ||
1469 | catch (AccessViolationException) | ||
1470 | { | ||
1471 | m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name); | ||
1472 | return; | ||
1473 | } | ||
1474 | } | ||
1475 | else | ||
1476 | { | ||
1477 | // _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
1478 | try | ||
1479 | { | ||
1480 | //Console.WriteLine(" CreateGeom 2"); | ||
1481 | SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); | ||
1482 | m_expectedCollisionContacts = 4; | ||
1483 | } | ||
1484 | catch (AccessViolationException) | ||
1485 | { | ||
1486 | m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name); | ||
1487 | return; | ||
1488 | } | ||
1489 | } | ||
1490 | } | ||
1491 | else | ||
1492 | { | ||
1493 | // _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
1494 | try | ||
1495 | { | ||
1496 | //Console.WriteLine(" CreateGeom 3"); | ||
1497 | SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); | ||
1498 | m_expectedCollisionContacts = 4; | ||
1499 | } | ||
1500 | catch (AccessViolationException) | ||
1501 | { | ||
1502 | m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name); | ||
1503 | return; | ||
1504 | } | ||
1505 | } | ||
1506 | } | ||
1507 | else | ||
1508 | { | ||
1509 | // _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
1510 | try | ||
1511 | { | ||
1512 | //Console.WriteLine(" CreateGeom 4"); | ||
1513 | SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); | ||
1514 | m_expectedCollisionContacts = 4; | ||
1515 | } | ||
1516 | catch (AccessViolationException) | ||
1517 | { | ||
1518 | m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name); | ||
1519 | return; | ||
1520 | } | ||
1521 | } | ||
1522 | } | ||
1523 | } | ||
1524 | |||
1525 | /// <summary> | ||
1526 | /// Remove the existing geom from this prim. | ||
1527 | /// </summary> | ||
1528 | /// <param name="m_targetSpace"></param> | ||
1529 | /// <param name="mesh">If null, then a mesh is used that is based on the profile shape data.</param> | ||
1530 | /// <returns>true if the geom was successfully removed, false if it was already gone or the remove failed.</returns> | ||
1531 | internal bool RemoveGeom() | ||
1532 | { | ||
1533 | if (prim_geom != IntPtr.Zero) | ||
1534 | { | ||
1535 | try | ||
1536 | { | ||
1537 | _parent_scene.geom_name_map.Remove(prim_geom); | ||
1538 | _parent_scene.actor_name_map.Remove(prim_geom); | ||
1539 | d.GeomDestroy(prim_geom); | ||
1540 | m_expectedCollisionContacts = 0; | ||
1541 | prim_geom = IntPtr.Zero; | ||
1542 | } | ||
1543 | catch (System.AccessViolationException) | ||
1544 | { | ||
1545 | prim_geom = IntPtr.Zero; | ||
1546 | m_expectedCollisionContacts = 0; | ||
1547 | m_log.ErrorFormat("[PHYSICS]: PrimGeom dead for {0}", Name); | ||
1548 | |||
1549 | return false; | ||
1550 | } | ||
1551 | |||
1552 | return true; | ||
1553 | } | ||
1554 | else | ||
1555 | { | ||
1556 | m_log.WarnFormat( | ||
1557 | "[ODE PRIM]: Called RemoveGeom() on {0} {1} where geometry was already null.", Name, LocalID); | ||
1558 | |||
1559 | return false; | ||
1560 | } | ||
1561 | } | ||
1562 | /// <summary> | ||
1563 | /// Add prim in response to an add taint. | ||
1564 | /// </summary> | ||
1565 | private void changeadd() | ||
1566 | { | ||
1567 | // m_log.DebugFormat("[ODE PRIM]: Adding prim {0}", Name); | ||
1568 | |||
1569 | int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position); | ||
1570 | IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position); | ||
1571 | |||
1572 | if (targetspace == IntPtr.Zero) | ||
1573 | targetspace = _parent_scene.createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]); | ||
1574 | |||
1575 | m_targetSpace = targetspace; | ||
1576 | |||
1577 | IMesh mesh = null; | ||
1578 | |||
1579 | if (_parent_scene.needsMeshing(_pbs)) | ||
1580 | { | ||
1581 | // Don't need to re-enable body.. it's done in SetMesh | ||
1582 | mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical); | ||
1583 | // createmesh returns null when it's a shape that isn't a cube. | ||
1584 | // m_log.Debug(m_localID); | ||
1585 | if (mesh == null) | ||
1586 | CheckMeshAsset(); | ||
1587 | else | ||
1588 | m_assetFailed = false; | ||
1589 | } | ||
1590 | |||
1591 | #if SPAM | ||
1592 | Console.WriteLine("changeadd 1"); | ||
1593 | #endif | ||
1594 | CreateGeom(m_targetSpace, mesh); | ||
1595 | |||
1596 | d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); | ||
1597 | d.Quaternion myrot = new d.Quaternion(); | ||
1598 | myrot.X = _orientation.X; | ||
1599 | myrot.Y = _orientation.Y; | ||
1600 | myrot.Z = _orientation.Z; | ||
1601 | myrot.W = _orientation.W; | ||
1602 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
1603 | |||
1604 | if (IsPhysical && Body == IntPtr.Zero) | ||
1605 | enableBody(); | ||
1606 | |||
1607 | changeSelectedStatus(); | ||
1608 | |||
1609 | m_taintadd = false; | ||
1610 | } | ||
1611 | |||
1612 | /// <summary> | ||
1613 | /// Move prim in response to a move taint. | ||
1614 | /// </summary> | ||
1615 | private void changemove() | ||
1616 | { | ||
1617 | if (IsPhysical) | ||
1618 | { | ||
1619 | if (!m_disabled && !m_taintremove && !childPrim) | ||
1620 | { | ||
1621 | if (Body == IntPtr.Zero) | ||
1622 | enableBody(); | ||
1623 | |||
1624 | //Prim auto disable after 20 frames, | ||
1625 | //if you move it, re-enable the prim manually. | ||
1626 | if (_parent != null) | ||
1627 | { | ||
1628 | if (m_linkJoint != IntPtr.Zero) | ||
1629 | { | ||
1630 | d.JointDestroy(m_linkJoint); | ||
1631 | m_linkJoint = IntPtr.Zero; | ||
1632 | } | ||
1633 | } | ||
1634 | |||
1635 | if (Body != IntPtr.Zero) | ||
1636 | { | ||
1637 | d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); | ||
1638 | |||
1639 | if (_parent != null) | ||
1640 | { | ||
1641 | OdePrim odParent = (OdePrim)_parent; | ||
1642 | if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body) | ||
1643 | { | ||
1644 | // KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used?? | ||
1645 | Console.WriteLine(" JointCreateFixed"); | ||
1646 | m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup); | ||
1647 | d.JointAttach(m_linkJoint, Body, odParent.Body); | ||
1648 | d.JointSetFixed(m_linkJoint); | ||
1649 | } | ||
1650 | } | ||
1651 | d.BodyEnable(Body); | ||
1652 | if (m_vehicle.Type != Vehicle.TYPE_NONE) | ||
1653 | { | ||
1654 | m_vehicle.Enable(Body, _parent_scene); | ||
1655 | } | ||
1656 | } | ||
1657 | else | ||
1658 | { | ||
1659 | m_log.WarnFormat("[PHYSICS]: Body for {0} still null after enableBody(). This is a crash scenario.", Name); | ||
1660 | } | ||
1661 | } | ||
1662 | //else | ||
1663 | // { | ||
1664 | //m_log.Debug("[BUG]: race!"); | ||
1665 | //} | ||
1666 | } | ||
1667 | |||
1668 | // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); | ||
1669 | // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); | ||
1670 | // _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
1671 | |||
1672 | IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, _position, m_targetSpace); | ||
1673 | m_targetSpace = tempspace; | ||
1674 | |||
1675 | // _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
1676 | |||
1677 | d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); | ||
1678 | |||
1679 | // _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
1680 | d.SpaceAdd(m_targetSpace, prim_geom); | ||
1681 | |||
1682 | changeSelectedStatus(); | ||
1683 | |||
1684 | resetCollisionAccounting(); | ||
1685 | m_taintposition = _position; | ||
1686 | } | ||
1687 | |||
1688 | internal void Move(float timestep) | ||
1689 | { | ||
1690 | float fx = 0; | ||
1691 | float fy = 0; | ||
1692 | float fz = 0; | ||
1693 | |||
1694 | if (outofBounds) | ||
1695 | return; | ||
1696 | |||
1697 | if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims. | ||
1698 | { | ||
1699 | if (m_vehicle.Type != Vehicle.TYPE_NONE) | ||
1700 | { | ||
1701 | // 'VEHICLES' are dealt with in ODEDynamics.cs | ||
1702 | m_vehicle.Step(timestep, _parent_scene); | ||
1703 | } | ||
1704 | else | ||
1705 | { | ||
1706 | //Console.WriteLine("Move " + Name); | ||
1707 | if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 | ||
1708 | // NON-'VEHICLES' are dealt with here | ||
1709 | // if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f)) | ||
1710 | // { | ||
1711 | // d.Vector3 avel2 = d.BodyGetAngularVel(Body); | ||
1712 | // /* | ||
1713 | // if (m_angularlock.X == 1) | ||
1714 | // avel2.X = 0; | ||
1715 | // if (m_angularlock.Y == 1) | ||
1716 | // avel2.Y = 0; | ||
1717 | // if (m_angularlock.Z == 1) | ||
1718 | // avel2.Z = 0; | ||
1719 | // d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z); | ||
1720 | // */ | ||
1721 | // } | ||
1722 | //float PID_P = 900.0f; | ||
1723 | |||
1724 | float m_mass = CalculateMass(); | ||
1725 | |||
1726 | // fz = 0f; | ||
1727 | //m_log.Info(m_collisionFlags.ToString()); | ||
1728 | |||
1729 | |||
1730 | //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. | ||
1731 | // would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ?? | ||
1732 | // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up | ||
1733 | // gravityz multiplier = 1 - m_buoyancy | ||
1734 | fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass; | ||
1735 | |||
1736 | if (PIDActive) | ||
1737 | { | ||
1738 | //Console.WriteLine("PID " + Name); | ||
1739 | // KF - this is for object move? eg. llSetPos() ? | ||
1740 | //if (!d.BodyIsEnabled(Body)) | ||
1741 | //d.BodySetForce(Body, 0f, 0f, 0f); | ||
1742 | // If we're using the PID controller, then we have no gravity | ||
1743 | //fz = (-1 * _parent_scene.gravityz) * m_mass; //KF: ?? Prims have no global gravity,so simply... | ||
1744 | fz = 0f; | ||
1745 | |||
1746 | // no lock; for now it's only called from within Simulate() | ||
1747 | |||
1748 | // If the PID Controller isn't active then we set our force | ||
1749 | // calculating base velocity to the current position | ||
1750 | |||
1751 | if ((m_PIDTau < 1) && (m_PIDTau != 0)) | ||
1752 | { | ||
1753 | //PID_G = PID_G / m_PIDTau; | ||
1754 | m_PIDTau = 1; | ||
1755 | } | ||
1756 | |||
1757 | if ((PID_G - m_PIDTau) <= 0) | ||
1758 | { | ||
1759 | PID_G = m_PIDTau + 1; | ||
1760 | } | ||
1761 | //PidStatus = true; | ||
1762 | |||
1763 | // PhysicsVector vec = new PhysicsVector(); | ||
1764 | d.Vector3 vel = d.BodyGetLinearVel(Body); | ||
1765 | |||
1766 | d.Vector3 pos = d.BodyGetPosition(Body); | ||
1767 | _target_velocity = | ||
1768 | new Vector3( | ||
1769 | (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), | ||
1770 | (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), | ||
1771 | (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) | ||
1772 | ); | ||
1773 | |||
1774 | // if velocity is zero, use position control; otherwise, velocity control | ||
1775 | |||
1776 | if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f)) | ||
1777 | { | ||
1778 | // keep track of where we stopped. No more slippin' & slidin' | ||
1779 | |||
1780 | // We only want to deactivate the PID Controller if we think we want to have our surrogate | ||
1781 | // react to the physics scene by moving it's position. | ||
1782 | // Avatar to Avatar collisions | ||
1783 | // Prim to avatar collisions | ||
1784 | |||
1785 | //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); | ||
1786 | //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); | ||
1787 | //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; | ||
1788 | d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); | ||
1789 | d.BodySetLinearVel(Body, 0, 0, 0); | ||
1790 | d.BodyAddForce(Body, 0, 0, fz); | ||
1791 | return; | ||
1792 | } | ||
1793 | else | ||
1794 | { | ||
1795 | _zeroFlag = false; | ||
1796 | |||
1797 | // We're flying and colliding with something | ||
1798 | fx = ((_target_velocity.X) - vel.X) * (PID_D); | ||
1799 | fy = ((_target_velocity.Y) - vel.Y) * (PID_D); | ||
1800 | |||
1801 | // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; | ||
1802 | |||
1803 | fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); | ||
1804 | } | ||
1805 | } // end if (PIDActive) | ||
1806 | |||
1807 | // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller | ||
1808 | if (m_useHoverPID && !PIDActive) | ||
1809 | { | ||
1810 | //Console.WriteLine("Hover " + Name); | ||
1811 | |||
1812 | // If we're using the PID controller, then we have no gravity | ||
1813 | fz = (-1 * _parent_scene.gravityz) * m_mass; | ||
1814 | |||
1815 | // no lock; for now it's only called from within Simulate() | ||
1816 | |||
1817 | // If the PID Controller isn't active then we set our force | ||
1818 | // calculating base velocity to the current position | ||
1819 | |||
1820 | if ((m_PIDTau < 1)) | ||
1821 | { | ||
1822 | PID_G = PID_G / m_PIDTau; | ||
1823 | } | ||
1824 | |||
1825 | if ((PID_G - m_PIDTau) <= 0) | ||
1826 | { | ||
1827 | PID_G = m_PIDTau + 1; | ||
1828 | } | ||
1829 | |||
1830 | // Where are we, and where are we headed? | ||
1831 | d.Vector3 pos = d.BodyGetPosition(Body); | ||
1832 | d.Vector3 vel = d.BodyGetLinearVel(Body); | ||
1833 | |||
1834 | // Non-Vehicles have a limited set of Hover options. | ||
1835 | // determine what our target height really is based on HoverType | ||
1836 | switch (m_PIDHoverType) | ||
1837 | { | ||
1838 | case PIDHoverType.Ground: | ||
1839 | m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); | ||
1840 | m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; | ||
1841 | break; | ||
1842 | case PIDHoverType.GroundAndWater: | ||
1843 | m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); | ||
1844 | m_waterHeight = _parent_scene.GetWaterLevel(); | ||
1845 | if (m_groundHeight > m_waterHeight) | ||
1846 | { | ||
1847 | m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; | ||
1848 | } | ||
1849 | else | ||
1850 | { | ||
1851 | m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; | ||
1852 | } | ||
1853 | break; | ||
1854 | |||
1855 | } // end switch (m_PIDHoverType) | ||
1856 | |||
1857 | |||
1858 | _target_velocity = | ||
1859 | new Vector3(0.0f, 0.0f, | ||
1860 | (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) | ||
1861 | ); | ||
1862 | |||
1863 | // if velocity is zero, use position control; otherwise, velocity control | ||
1864 | |||
1865 | if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) | ||
1866 | { | ||
1867 | // keep track of where we stopped. No more slippin' & slidin' | ||
1868 | |||
1869 | // We only want to deactivate the PID Controller if we think we want to have our surrogate | ||
1870 | // react to the physics scene by moving it's position. | ||
1871 | // Avatar to Avatar collisions | ||
1872 | // Prim to avatar collisions | ||
1873 | |||
1874 | d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); | ||
1875 | d.BodySetLinearVel(Body, vel.X, vel.Y, 0); | ||
1876 | d.BodyAddForce(Body, 0, 0, fz); | ||
1877 | return; | ||
1878 | } | ||
1879 | else | ||
1880 | { | ||
1881 | _zeroFlag = false; | ||
1882 | |||
1883 | // We're flying and colliding with something | ||
1884 | fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); | ||
1885 | } | ||
1886 | } | ||
1887 | |||
1888 | fx *= m_mass; | ||
1889 | fy *= m_mass; | ||
1890 | //fz *= m_mass; | ||
1891 | |||
1892 | fx += m_force.X; | ||
1893 | fy += m_force.Y; | ||
1894 | fz += m_force.Z; | ||
1895 | |||
1896 | //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); | ||
1897 | if (fx != 0 || fy != 0 || fz != 0) | ||
1898 | { | ||
1899 | //m_taintdisable = true; | ||
1900 | //base.RaiseOutOfBounds(Position); | ||
1901 | //d.BodySetLinearVel(Body, fx, fy, 0f); | ||
1902 | if (!d.BodyIsEnabled(Body)) | ||
1903 | { | ||
1904 | // A physical body at rest on a surface will auto-disable after a while, | ||
1905 | // this appears to re-enable it incase the surface it is upon vanishes, | ||
1906 | // and the body should fall again. | ||
1907 | d.BodySetLinearVel(Body, 0f, 0f, 0f); | ||
1908 | d.BodySetForce(Body, 0, 0, 0); | ||
1909 | enableBodySoft(); | ||
1910 | } | ||
1911 | |||
1912 | // 35x10 = 350n times the mass per second applied maximum. | ||
1913 | float nmax = 35f * m_mass; | ||
1914 | float nmin = -35f * m_mass; | ||
1915 | |||
1916 | if (fx > nmax) | ||
1917 | fx = nmax; | ||
1918 | if (fx < nmin) | ||
1919 | fx = nmin; | ||
1920 | if (fy > nmax) | ||
1921 | fy = nmax; | ||
1922 | if (fy < nmin) | ||
1923 | fy = nmin; | ||
1924 | d.BodyAddForce(Body, fx, fy, fz); | ||
1925 | //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); | ||
1926 | } | ||
1927 | } | ||
1928 | } | ||
1929 | else | ||
1930 | { // is not physical, or is not a body or is selected | ||
1931 | // _zeroPosition = d.BodyGetPosition(Body); | ||
1932 | return; | ||
1933 | //Console.WriteLine("Nothing " + Name); | ||
1934 | |||
1935 | } | ||
1936 | } | ||
1937 | |||
1938 | private void rotate() | ||
1939 | { | ||
1940 | d.Quaternion myrot = new d.Quaternion(); | ||
1941 | myrot.X = _orientation.X; | ||
1942 | myrot.Y = _orientation.Y; | ||
1943 | myrot.Z = _orientation.Z; | ||
1944 | myrot.W = _orientation.W; | ||
1945 | if (Body != IntPtr.Zero) | ||
1946 | { | ||
1947 | // KF: If this is a root prim do BodySet | ||
1948 | d.BodySetQuaternion(Body, ref myrot); | ||
1949 | if (IsPhysical) | ||
1950 | { | ||
1951 | if (!m_angularlock.ApproxEquals(Vector3.One, 0f)) | ||
1952 | createAMotor(m_angularlock); | ||
1953 | } | ||
1954 | } | ||
1955 | else | ||
1956 | { | ||
1957 | // daughter prim, do Geom set | ||
1958 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
1959 | } | ||
1960 | |||
1961 | resetCollisionAccounting(); | ||
1962 | m_taintrot = _orientation; | ||
1963 | } | ||
1964 | |||
1965 | private void resetCollisionAccounting() | ||
1966 | { | ||
1967 | m_collisionscore = 0; | ||
1968 | m_interpenetrationcount = 0; | ||
1969 | m_disabled = false; | ||
1970 | } | ||
1971 | |||
1972 | /// <summary> | ||
1973 | /// Change prim in response to a disable taint. | ||
1974 | /// </summary> | ||
1975 | private void changedisable() | ||
1976 | { | ||
1977 | m_disabled = true; | ||
1978 | if (Body != IntPtr.Zero) | ||
1979 | { | ||
1980 | d.BodyDisable(Body); | ||
1981 | Body = IntPtr.Zero; | ||
1982 | } | ||
1983 | |||
1984 | m_taintdisable = false; | ||
1985 | } | ||
1986 | |||
1987 | /// <summary> | ||
1988 | /// Change prim in response to a physics status taint | ||
1989 | /// </summary> | ||
1990 | private void changePhysicsStatus() | ||
1991 | { | ||
1992 | if (IsPhysical) | ||
1993 | { | ||
1994 | if (Body == IntPtr.Zero) | ||
1995 | { | ||
1996 | if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) | ||
1997 | { | ||
1998 | changeshape(); | ||
1999 | } | ||
2000 | else | ||
2001 | { | ||
2002 | enableBody(); | ||
2003 | } | ||
2004 | } | ||
2005 | } | ||
2006 | else | ||
2007 | { | ||
2008 | if (Body != IntPtr.Zero) | ||
2009 | { | ||
2010 | if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) | ||
2011 | { | ||
2012 | RemoveGeom(); | ||
2013 | |||
2014 | //Console.WriteLine("changePhysicsStatus for " + Name); | ||
2015 | changeadd(); | ||
2016 | } | ||
2017 | |||
2018 | if (childPrim) | ||
2019 | { | ||
2020 | if (_parent != null) | ||
2021 | { | ||
2022 | OdePrim parent = (OdePrim)_parent; | ||
2023 | parent.ChildDelink(this); | ||
2024 | } | ||
2025 | } | ||
2026 | else | ||
2027 | { | ||
2028 | disableBody(); | ||
2029 | } | ||
2030 | } | ||
2031 | } | ||
2032 | |||
2033 | changeSelectedStatus(); | ||
2034 | |||
2035 | resetCollisionAccounting(); | ||
2036 | m_taintPhysics = IsPhysical; | ||
2037 | } | ||
2038 | |||
2039 | /// <summary> | ||
2040 | /// Change prim in response to a size taint. | ||
2041 | /// </summary> | ||
2042 | private void changesize() | ||
2043 | { | ||
2044 | #if SPAM | ||
2045 | m_log.DebugFormat("[ODE PRIM]: Called changesize"); | ||
2046 | #endif | ||
2047 | |||
2048 | if (_size.X <= 0) _size.X = 0.01f; | ||
2049 | if (_size.Y <= 0) _size.Y = 0.01f; | ||
2050 | if (_size.Z <= 0) _size.Z = 0.01f; | ||
2051 | |||
2052 | //kill body to rebuild | ||
2053 | if (IsPhysical && Body != IntPtr.Zero) | ||
2054 | { | ||
2055 | if (childPrim) | ||
2056 | { | ||
2057 | if (_parent != null) | ||
2058 | { | ||
2059 | OdePrim parent = (OdePrim)_parent; | ||
2060 | parent.ChildDelink(this); | ||
2061 | } | ||
2062 | } | ||
2063 | else | ||
2064 | { | ||
2065 | disableBody(); | ||
2066 | } | ||
2067 | } | ||
2068 | |||
2069 | if (d.SpaceQuery(m_targetSpace, prim_geom)) | ||
2070 | { | ||
2071 | // _parent_scene.waitForSpaceUnlock(m_targetSpace); | ||
2072 | d.SpaceRemove(m_targetSpace, prim_geom); | ||
2073 | } | ||
2074 | |||
2075 | RemoveGeom(); | ||
2076 | |||
2077 | // we don't need to do space calculation because the client sends a position update also. | ||
2078 | |||
2079 | IMesh mesh = null; | ||
2080 | |||
2081 | // Construction of new prim | ||
2082 | if (_parent_scene.needsMeshing(_pbs)) | ||
2083 | { | ||
2084 | float meshlod = _parent_scene.meshSculptLOD; | ||
2085 | |||
2086 | if (IsPhysical) | ||
2087 | meshlod = _parent_scene.MeshSculptphysicalLOD; | ||
2088 | // Don't need to re-enable body.. it's done in SetMesh | ||
2089 | |||
2090 | if (_parent_scene.needsMeshing(_pbs)) | ||
2091 | { | ||
2092 | mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical); | ||
2093 | if (mesh == null) | ||
2094 | CheckMeshAsset(); | ||
2095 | else | ||
2096 | m_assetFailed = false; | ||
2097 | } | ||
2098 | |||
2099 | } | ||
2100 | |||
2101 | CreateGeom(m_targetSpace, mesh); | ||
2102 | d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); | ||
2103 | d.Quaternion myrot = new d.Quaternion(); | ||
2104 | myrot.X = _orientation.X; | ||
2105 | myrot.Y = _orientation.Y; | ||
2106 | myrot.Z = _orientation.Z; | ||
2107 | myrot.W = _orientation.W; | ||
2108 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
2109 | |||
2110 | //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z); | ||
2111 | if (IsPhysical && Body == IntPtr.Zero && !childPrim) | ||
2112 | { | ||
2113 | // Re creates body on size. | ||
2114 | // EnableBody also does setMass() | ||
2115 | enableBody(); | ||
2116 | d.BodyEnable(Body); | ||
2117 | } | ||
2118 | |||
2119 | changeSelectedStatus(); | ||
2120 | |||
2121 | if (childPrim) | ||
2122 | { | ||
2123 | if (_parent is OdePrim) | ||
2124 | { | ||
2125 | OdePrim parent = (OdePrim)_parent; | ||
2126 | parent.ChildSetGeom(this); | ||
2127 | } | ||
2128 | } | ||
2129 | resetCollisionAccounting(); | ||
2130 | m_taintsize = _size; | ||
2131 | } | ||
2132 | |||
2133 | /// <summary> | ||
2134 | /// Change prim in response to a float on water taint. | ||
2135 | /// </summary> | ||
2136 | /// <param name="timestep"></param> | ||
2137 | private void changefloatonwater() | ||
2138 | { | ||
2139 | m_collidesWater = m_taintCollidesWater; | ||
2140 | |||
2141 | if (m_collidesWater) | ||
2142 | { | ||
2143 | m_collisionFlags |= CollisionCategories.Water; | ||
2144 | } | ||
2145 | else | ||
2146 | { | ||
2147 | m_collisionFlags &= ~CollisionCategories.Water; | ||
2148 | } | ||
2149 | |||
2150 | if (m_assetFailed) | ||
2151 | d.GeomSetCollideBits(prim_geom, BadMeshAssetCollideBits); | ||
2152 | else | ||
2153 | |||
2154 | d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); | ||
2155 | } | ||
2156 | /// <summary> | ||
2157 | /// Change prim in response to a shape taint. | ||
2158 | /// </summary> | ||
2159 | private void changeshape() | ||
2160 | { | ||
2161 | m_taintshape = false; | ||
2162 | |||
2163 | // Cleanup of old prim geometry and Bodies | ||
2164 | if (IsPhysical && Body != IntPtr.Zero) | ||
2165 | { | ||
2166 | if (childPrim) | ||
2167 | { | ||
2168 | if (_parent != null) | ||
2169 | { | ||
2170 | OdePrim parent = (OdePrim)_parent; | ||
2171 | parent.ChildDelink(this); | ||
2172 | } | ||
2173 | } | ||
2174 | else | ||
2175 | { | ||
2176 | disableBody(); | ||
2177 | } | ||
2178 | } | ||
2179 | |||
2180 | RemoveGeom(); | ||
2181 | |||
2182 | // we don't need to do space calculation because the client sends a position update also. | ||
2183 | if (_size.X <= 0) _size.X = 0.01f; | ||
2184 | if (_size.Y <= 0) _size.Y = 0.01f; | ||
2185 | if (_size.Z <= 0) _size.Z = 0.01f; | ||
2186 | // Construction of new prim | ||
2187 | |||
2188 | IMesh mesh = null; | ||
2189 | |||
2190 | |||
2191 | if (_parent_scene.needsMeshing(_pbs)) | ||
2192 | { | ||
2193 | // Don't need to re-enable body.. it's done in CreateMesh | ||
2194 | float meshlod = _parent_scene.meshSculptLOD; | ||
2195 | |||
2196 | if (IsPhysical) | ||
2197 | meshlod = _parent_scene.MeshSculptphysicalLOD; | ||
2198 | |||
2199 | // createmesh returns null when it doesn't mesh. | ||
2200 | mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical); | ||
2201 | if (mesh == null) | ||
2202 | CheckMeshAsset(); | ||
2203 | else | ||
2204 | m_assetFailed = false; | ||
2205 | } | ||
2206 | |||
2207 | CreateGeom(m_targetSpace, mesh); | ||
2208 | d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); | ||
2209 | d.Quaternion myrot = new d.Quaternion(); | ||
2210 | //myrot.W = _orientation.w; | ||
2211 | myrot.W = _orientation.W; | ||
2212 | myrot.X = _orientation.X; | ||
2213 | myrot.Y = _orientation.Y; | ||
2214 | myrot.Z = _orientation.Z; | ||
2215 | d.GeomSetQuaternion(prim_geom, ref myrot); | ||
2216 | |||
2217 | //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z); | ||
2218 | if (IsPhysical && Body == IntPtr.Zero) | ||
2219 | { | ||
2220 | // Re creates body on size. | ||
2221 | // EnableBody also does setMass() | ||
2222 | enableBody(); | ||
2223 | if (Body != IntPtr.Zero) | ||
2224 | { | ||
2225 | d.BodyEnable(Body); | ||
2226 | } | ||
2227 | } | ||
2228 | |||
2229 | changeSelectedStatus(); | ||
2230 | |||
2231 | if (childPrim) | ||
2232 | { | ||
2233 | if (_parent is OdePrim) | ||
2234 | { | ||
2235 | OdePrim parent = (OdePrim)_parent; | ||
2236 | parent.ChildSetGeom(this); | ||
2237 | } | ||
2238 | } | ||
2239 | |||
2240 | resetCollisionAccounting(); | ||
2241 | // m_taintshape = false; | ||
2242 | } | ||
2243 | |||
2244 | /// <summary> | ||
2245 | /// Change prim in response to an add force taint. | ||
2246 | /// </summary> | ||
2247 | private void changeAddForce() | ||
2248 | { | ||
2249 | if (!m_isSelected) | ||
2250 | { | ||
2251 | lock (m_forcelist) | ||
2252 | { | ||
2253 | //m_log.Info("[PHYSICS]: dequeing forcelist"); | ||
2254 | if (IsPhysical) | ||
2255 | { | ||
2256 | Vector3 iforce = Vector3.Zero; | ||
2257 | int i = 0; | ||
2258 | try | ||
2259 | { | ||
2260 | for (i = 0; i < m_forcelist.Count; i++) | ||
2261 | { | ||
2262 | |||
2263 | iforce = iforce + (m_forcelist[i] * 100); | ||
2264 | } | ||
2265 | } | ||
2266 | catch (IndexOutOfRangeException) | ||
2267 | { | ||
2268 | m_forcelist = new List<Vector3>(); | ||
2269 | m_collisionscore = 0; | ||
2270 | m_interpenetrationcount = 0; | ||
2271 | m_taintforce = false; | ||
2272 | return; | ||
2273 | } | ||
2274 | catch (ArgumentOutOfRangeException) | ||
2275 | { | ||
2276 | m_forcelist = new List<Vector3>(); | ||
2277 | m_collisionscore = 0; | ||
2278 | m_interpenetrationcount = 0; | ||
2279 | m_taintforce = false; | ||
2280 | return; | ||
2281 | } | ||
2282 | d.BodyEnable(Body); | ||
2283 | d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z); | ||
2284 | } | ||
2285 | m_forcelist.Clear(); | ||
2286 | } | ||
2287 | |||
2288 | m_collisionscore = 0; | ||
2289 | m_interpenetrationcount = 0; | ||
2290 | } | ||
2291 | |||
2292 | m_taintforce = false; | ||
2293 | } | ||
2294 | |||
2295 | /// <summary> | ||
2296 | /// Change prim in response to a torque taint. | ||
2297 | /// </summary> | ||
2298 | private void changeSetTorque() | ||
2299 | { | ||
2300 | if (!m_isSelected) | ||
2301 | { | ||
2302 | if (IsPhysical && Body != IntPtr.Zero) | ||
2303 | { | ||
2304 | d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z); | ||
2305 | } | ||
2306 | } | ||
2307 | |||
2308 | m_taintTorque = Vector3.Zero; | ||
2309 | } | ||
2310 | |||
2311 | /// <summary> | ||
2312 | /// Change prim in response to an angular force taint. | ||
2313 | /// </summary> | ||
2314 | private void changeAddAngularForce() | ||
2315 | { | ||
2316 | if (!m_isSelected) | ||
2317 | { | ||
2318 | lock (m_angularforcelist) | ||
2319 | { | ||
2320 | //m_log.Info("[PHYSICS]: dequeing forcelist"); | ||
2321 | if (IsPhysical) | ||
2322 | { | ||
2323 | Vector3 iforce = Vector3.Zero; | ||
2324 | for (int i = 0; i < m_angularforcelist.Count; i++) | ||
2325 | { | ||
2326 | iforce = iforce + (m_angularforcelist[i] * 100); | ||
2327 | } | ||
2328 | d.BodyEnable(Body); | ||
2329 | d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z); | ||
2330 | |||
2331 | } | ||
2332 | m_angularforcelist.Clear(); | ||
2333 | } | ||
2334 | |||
2335 | m_collisionscore = 0; | ||
2336 | m_interpenetrationcount = 0; | ||
2337 | } | ||
2338 | |||
2339 | m_taintaddangularforce = false; | ||
2340 | } | ||
2341 | |||
2342 | /// <summary> | ||
2343 | /// Change prim in response to a velocity taint. | ||
2344 | /// </summary> | ||
2345 | private void changevelocity() | ||
2346 | { | ||
2347 | if (!m_isSelected) | ||
2348 | { | ||
2349 | // Not sure exactly why this sleep is here, but from experimentation it appears to stop an avatar | ||
2350 | // walking through a default rez size prim if it keeps kicking it around - justincc. | ||
2351 | Thread.Sleep(20); | ||
2352 | |||
2353 | if (IsPhysical) | ||
2354 | { | ||
2355 | if (Body != IntPtr.Zero) | ||
2356 | { | ||
2357 | d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z); | ||
2358 | } | ||
2359 | } | ||
2360 | |||
2361 | //resetCollisionAccounting(); | ||
2362 | } | ||
2363 | |||
2364 | m_taintVelocity = Vector3.Zero; | ||
2365 | } | ||
2366 | |||
2367 | internal void setPrimForRemoval() | ||
2368 | { | ||
2369 | m_taintremove = true; | ||
2370 | } | ||
2371 | |||
2372 | public override bool Flying | ||
2373 | { | ||
2374 | // no flying prims for you | ||
2375 | get { return false; } | ||
2376 | set { } | ||
2377 | } | ||
2378 | |||
2379 | public override bool IsColliding | ||
2380 | { | ||
2381 | get { return iscolliding; } | ||
2382 | set { iscolliding = value; } | ||
2383 | } | ||
2384 | |||
2385 | public override bool CollidingGround | ||
2386 | { | ||
2387 | get { return false; } | ||
2388 | set { return; } | ||
2389 | } | ||
2390 | |||
2391 | public override bool CollidingObj | ||
2392 | { | ||
2393 | get { return false; } | ||
2394 | set { return; } | ||
2395 | } | ||
2396 | |||
2397 | public override bool ThrottleUpdates | ||
2398 | { | ||
2399 | get { return m_throttleUpdates; } | ||
2400 | set { m_throttleUpdates = value; } | ||
2401 | } | ||
2402 | |||
2403 | public override bool Stopped | ||
2404 | { | ||
2405 | get { return _zeroFlag; } | ||
2406 | } | ||
2407 | |||
2408 | public override Vector3 Position | ||
2409 | { | ||
2410 | get { return _position; } | ||
2411 | |||
2412 | set { _position = value; | ||
2413 | //m_log.Info("[PHYSICS]: " + _position.ToString()); | ||
2414 | } | ||
2415 | } | ||
2416 | |||
2417 | public override Vector3 Size | ||
2418 | { | ||
2419 | get { return _size; } | ||
2420 | set | ||
2421 | { | ||
2422 | if (value.IsFinite()) | ||
2423 | { | ||
2424 | _size = value; | ||
2425 | // m_log.DebugFormat("[PHYSICS]: Set size on {0} to {1}", Name, value); | ||
2426 | } | ||
2427 | else | ||
2428 | { | ||
2429 | m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name); | ||
2430 | } | ||
2431 | } | ||
2432 | } | ||
2433 | |||
2434 | public override float Mass | ||
2435 | { | ||
2436 | get { return CalculateMass(); } | ||
2437 | } | ||
2438 | |||
2439 | public override Vector3 Force | ||
2440 | { | ||
2441 | //get { return Vector3.Zero; } | ||
2442 | get { return m_force; } | ||
2443 | set | ||
2444 | { | ||
2445 | if (value.IsFinite()) | ||
2446 | { | ||
2447 | m_force = value; | ||
2448 | } | ||
2449 | else | ||
2450 | { | ||
2451 | m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name); | ||
2452 | } | ||
2453 | } | ||
2454 | } | ||
2455 | |||
2456 | public override int VehicleType | ||
2457 | { | ||
2458 | get { return (int)m_vehicle.Type; } | ||
2459 | set { m_vehicle.ProcessTypeChange((Vehicle)value); } | ||
2460 | } | ||
2461 | |||
2462 | public override void VehicleFloatParam(int param, float value) | ||
2463 | { | ||
2464 | m_vehicle.ProcessFloatVehicleParam((Vehicle) param, value); | ||
2465 | } | ||
2466 | |||
2467 | public override void VehicleVectorParam(int param, Vector3 value) | ||
2468 | { | ||
2469 | m_vehicle.ProcessVectorVehicleParam((Vehicle) param, value); | ||
2470 | } | ||
2471 | |||
2472 | public override void VehicleRotationParam(int param, Quaternion rotation) | ||
2473 | { | ||
2474 | m_vehicle.ProcessRotationVehicleParam((Vehicle) param, rotation); | ||
2475 | } | ||
2476 | |||
2477 | public override void VehicleFlags(int param, bool remove) | ||
2478 | { | ||
2479 | m_vehicle.ProcessVehicleFlags(param, remove); | ||
2480 | } | ||
2481 | |||
2482 | public override void SetVolumeDetect(int param) | ||
2483 | { | ||
2484 | // We have to lock the scene here so that an entire simulate loop either uses volume detect for all | ||
2485 | // possible collisions with this prim or for none of them. | ||
2486 | lock (_parent_scene.OdeLock) | ||
2487 | { | ||
2488 | m_isVolumeDetect = (param != 0); | ||
2489 | } | ||
2490 | } | ||
2491 | |||
2492 | public override Vector3 CenterOfMass | ||
2493 | { | ||
2494 | get { return Vector3.Zero; } | ||
2495 | } | ||
2496 | |||
2497 | public override Vector3 GeometricCenter | ||
2498 | { | ||
2499 | get { return Vector3.Zero; } | ||
2500 | } | ||
2501 | |||
2502 | public override PrimitiveBaseShape Shape | ||
2503 | { | ||
2504 | set | ||
2505 | { | ||
2506 | _pbs = value; | ||
2507 | m_assetFailed = false; | ||
2508 | m_taintshape = true; | ||
2509 | } | ||
2510 | } | ||
2511 | |||
2512 | public override Vector3 Velocity | ||
2513 | { | ||
2514 | get | ||
2515 | { | ||
2516 | // Average previous velocity with the new one so | ||
2517 | // client object interpolation works a 'little' better | ||
2518 | if (_zeroFlag) | ||
2519 | return Vector3.Zero; | ||
2520 | |||
2521 | Vector3 returnVelocity = Vector3.Zero; | ||
2522 | returnVelocity.X = (m_lastVelocity.X + _velocity.X) * 0.5f; // 0.5f is mathematically equiv to '/ 2' | ||
2523 | returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) * 0.5f; | ||
2524 | returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) * 0.5f; | ||
2525 | return returnVelocity; | ||
2526 | } | ||
2527 | set | ||
2528 | { | ||
2529 | if (value.IsFinite()) | ||
2530 | { | ||
2531 | _velocity = value; | ||
2532 | |||
2533 | m_taintVelocity = value; | ||
2534 | _parent_scene.AddPhysicsActorTaint(this); | ||
2535 | } | ||
2536 | else | ||
2537 | { | ||
2538 | m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name); | ||
2539 | } | ||
2540 | |||
2541 | } | ||
2542 | } | ||
2543 | |||
2544 | public override Vector3 Torque | ||
2545 | { | ||
2546 | get | ||
2547 | { | ||
2548 | if (!IsPhysical || Body == IntPtr.Zero) | ||
2549 | return Vector3.Zero; | ||
2550 | |||
2551 | return _torque; | ||
2552 | } | ||
2553 | |||
2554 | set | ||
2555 | { | ||
2556 | if (value.IsFinite()) | ||
2557 | { | ||
2558 | m_taintTorque = value; | ||
2559 | _parent_scene.AddPhysicsActorTaint(this); | ||
2560 | } | ||
2561 | else | ||
2562 | { | ||
2563 | m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name); | ||
2564 | } | ||
2565 | } | ||
2566 | } | ||
2567 | |||
2568 | public override float CollisionScore | ||
2569 | { | ||
2570 | get { return m_collisionscore; } | ||
2571 | set { m_collisionscore = value; } | ||
2572 | } | ||
2573 | |||
2574 | public override bool Kinematic | ||
2575 | { | ||
2576 | get { return false; } | ||
2577 | set { } | ||
2578 | } | ||
2579 | |||
2580 | public override Quaternion Orientation | ||
2581 | { | ||
2582 | get { return _orientation; } | ||
2583 | set | ||
2584 | { | ||
2585 | if (QuaternionIsFinite(value)) | ||
2586 | _orientation = value; | ||
2587 | else | ||
2588 | m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name); | ||
2589 | } | ||
2590 | } | ||
2591 | |||
2592 | private static bool QuaternionIsFinite(Quaternion q) | ||
2593 | { | ||
2594 | if (Single.IsNaN(q.X) || Single.IsInfinity(q.X)) | ||
2595 | return false; | ||
2596 | if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y)) | ||
2597 | return false; | ||
2598 | if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z)) | ||
2599 | return false; | ||
2600 | if (Single.IsNaN(q.W) || Single.IsInfinity(q.W)) | ||
2601 | return false; | ||
2602 | return true; | ||
2603 | } | ||
2604 | |||
2605 | public override Vector3 Acceleration | ||
2606 | { | ||
2607 | get { return _acceleration; } | ||
2608 | set { _acceleration = value; } | ||
2609 | } | ||
2610 | |||
2611 | public override void AddForce(Vector3 force, bool pushforce) | ||
2612 | { | ||
2613 | if (force.IsFinite()) | ||
2614 | { | ||
2615 | lock (m_forcelist) | ||
2616 | m_forcelist.Add(force); | ||
2617 | |||
2618 | m_taintforce = true; | ||
2619 | } | ||
2620 | else | ||
2621 | { | ||
2622 | m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name); | ||
2623 | } | ||
2624 | //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString()); | ||
2625 | } | ||
2626 | |||
2627 | public override void AddAngularForce(Vector3 force, bool pushforce) | ||
2628 | { | ||
2629 | if (force.IsFinite()) | ||
2630 | { | ||
2631 | m_angularforcelist.Add(force); | ||
2632 | m_taintaddangularforce = true; | ||
2633 | } | ||
2634 | else | ||
2635 | { | ||
2636 | m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name); | ||
2637 | } | ||
2638 | } | ||
2639 | |||
2640 | public override Vector3 RotationalVelocity | ||
2641 | { | ||
2642 | get | ||
2643 | { | ||
2644 | Vector3 pv = Vector3.Zero; | ||
2645 | if (_zeroFlag) | ||
2646 | return pv; | ||
2647 | m_lastUpdateSent = false; | ||
2648 | |||
2649 | if (m_rotationalVelocity.ApproxEquals(pv, 0.2f)) | ||
2650 | return pv; | ||
2651 | |||
2652 | return m_rotationalVelocity; | ||
2653 | } | ||
2654 | set | ||
2655 | { | ||
2656 | if (value.IsFinite()) | ||
2657 | { | ||
2658 | m_rotationalVelocity = value; | ||
2659 | setAngularVelocity(value.X, value.Y, value.Z); | ||
2660 | } | ||
2661 | else | ||
2662 | { | ||
2663 | m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name); | ||
2664 | } | ||
2665 | } | ||
2666 | } | ||
2667 | |||
2668 | public override void CrossingFailure() | ||
2669 | { | ||
2670 | /* | ||
2671 | m_crossingfailures++; | ||
2672 | if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds) | ||
2673 | { | ||
2674 | base.RaiseOutOfBounds(_position); | ||
2675 | return; | ||
2676 | } | ||
2677 | else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds) | ||
2678 | { | ||
2679 | m_log.Warn("[PHYSICS]: Too many crossing failures for: " + Name); | ||
2680 | } | ||
2681 | */ | ||
2682 | _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f); | ||
2683 | _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f); | ||
2684 | _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f); | ||
2685 | |||
2686 | m_lastposition = _position; | ||
2687 | _velocity.X = 0; | ||
2688 | _velocity.Y = 0; | ||
2689 | _velocity.Z = 0; | ||
2690 | |||
2691 | m_lastVelocity = _velocity; | ||
2692 | |||
2693 | if (Body != IntPtr.Zero) | ||
2694 | { | ||
2695 | d.BodySetLinearVel(Body, 0, 0, 0); // stop it | ||
2696 | d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); | ||
2697 | } | ||
2698 | |||
2699 | outofBounds = false; | ||
2700 | base.RequestPhysicsterseUpdate(); | ||
2701 | |||
2702 | } | ||
2703 | |||
2704 | public override float Buoyancy | ||
2705 | { | ||
2706 | get { return m_buoyancy; } | ||
2707 | set { m_buoyancy = value; } | ||
2708 | } | ||
2709 | |||
2710 | public override void link(PhysicsActor obj) | ||
2711 | { | ||
2712 | m_taintparent = obj; | ||
2713 | } | ||
2714 | |||
2715 | public override void delink() | ||
2716 | { | ||
2717 | m_taintparent = null; | ||
2718 | } | ||
2719 | |||
2720 | public override void LockAngularMotion(Vector3 axis) | ||
2721 | { | ||
2722 | // reverse the zero/non zero values for ODE. | ||
2723 | if (axis.IsFinite()) | ||
2724 | { | ||
2725 | axis.X = (axis.X > 0) ? 1f : 0f; | ||
2726 | axis.Y = (axis.Y > 0) ? 1f : 0f; | ||
2727 | axis.Z = (axis.Z > 0) ? 1f : 0f; | ||
2728 | m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); | ||
2729 | m_taintAngularLock = axis; | ||
2730 | } | ||
2731 | else | ||
2732 | { | ||
2733 | m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name); | ||
2734 | } | ||
2735 | } | ||
2736 | |||
2737 | internal void UpdatePositionAndVelocity() | ||
2738 | { | ||
2739 | // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! | ||
2740 | if (outofBounds) | ||
2741 | return; | ||
2742 | if (_parent == null) | ||
2743 | { | ||
2744 | Vector3 pv = Vector3.Zero; | ||
2745 | bool lastZeroFlag = _zeroFlag; | ||
2746 | float m_minvelocity = 0; | ||
2747 | if (Body != (IntPtr)0) // FIXME -> or if it is a joint | ||
2748 | { | ||
2749 | d.Vector3 vec = d.BodyGetPosition(Body); | ||
2750 | d.Quaternion ori = d.BodyGetQuaternion(Body); | ||
2751 | d.Vector3 vel = d.BodyGetLinearVel(Body); | ||
2752 | d.Vector3 rotvel = d.BodyGetAngularVel(Body); | ||
2753 | d.Vector3 torque = d.BodyGetTorque(Body); | ||
2754 | _torque = new Vector3(torque.X, torque.Y, torque.Z); | ||
2755 | Vector3 l_position = Vector3.Zero; | ||
2756 | Quaternion l_orientation = Quaternion.Identity; | ||
2757 | |||
2758 | m_lastposition = _position; | ||
2759 | m_lastorientation = _orientation; | ||
2760 | |||
2761 | l_position.X = vec.X; | ||
2762 | l_position.Y = vec.Y; | ||
2763 | l_position.Z = vec.Z; | ||
2764 | l_orientation.X = ori.X; | ||
2765 | l_orientation.Y = ori.Y; | ||
2766 | l_orientation.Z = ori.Z; | ||
2767 | l_orientation.W = ori.W; | ||
2768 | |||
2769 | if (l_position.Z < 0) | ||
2770 | { | ||
2771 | // This is so prim that get lost underground don't fall forever and suck up | ||
2772 | // | ||
2773 | // Sim resources and memory. | ||
2774 | // Disables the prim's movement physics.... | ||
2775 | // It's a hack and will generate a console message if it fails. | ||
2776 | |||
2777 | //IsPhysical = false; | ||
2778 | |||
2779 | _acceleration.X = 0; | ||
2780 | _acceleration.Y = 0; | ||
2781 | _acceleration.Z = 0; | ||
2782 | |||
2783 | _velocity.X = 0; | ||
2784 | _velocity.Y = 0; | ||
2785 | _velocity.Z = 0; | ||
2786 | m_rotationalVelocity.X = 0; | ||
2787 | m_rotationalVelocity.Y = 0; | ||
2788 | m_rotationalVelocity.Z = 0; | ||
2789 | |||
2790 | if (_parent == null) | ||
2791 | base.RaiseOutOfBounds(_position); | ||
2792 | |||
2793 | if (_parent == null) | ||
2794 | base.RequestPhysicsterseUpdate(); | ||
2795 | |||
2796 | m_throttleUpdates = false; | ||
2797 | throttleCounter = 0; | ||
2798 | _zeroFlag = true; | ||
2799 | //outofBounds = true; | ||
2800 | return; | ||
2801 | } | ||
2802 | |||
2803 | if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || l_position.X < 0f || l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || l_position.Y < 0f) | ||
2804 | { | ||
2805 | //base.RaiseOutOfBounds(l_position); | ||
2806 | /* | ||
2807 | if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds) | ||
2808 | { | ||
2809 | _position = l_position; | ||
2810 | //_parent_scene.remActivePrim(this); | ||
2811 | if (_parent == null) | ||
2812 | base.RequestPhysicsterseUpdate(); | ||
2813 | return; | ||
2814 | } | ||
2815 | else | ||
2816 | { | ||
2817 | if (_parent == null) | ||
2818 | base.RaiseOutOfBounds(l_position); | ||
2819 | return; | ||
2820 | } | ||
2821 | */ | ||
2822 | outofBounds = true; | ||
2823 | // part near the border on outside | ||
2824 | if (l_position.X < 0) | ||
2825 | Util.Clamp(l_position.X, -0.1f, -2f); | ||
2826 | else | ||
2827 | Util.Clamp(l_position.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f); | ||
2828 | if (l_position.Y < 0) | ||
2829 | Util.Clamp(l_position.Y, -0.1f, -2f); | ||
2830 | else | ||
2831 | Util.Clamp(l_position.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f); | ||
2832 | |||
2833 | d.BodySetPosition(Body, l_position.X, l_position.Y, l_position.Z); | ||
2834 | |||
2835 | // stop it | ||
2836 | d.BodySetAngularVel(Body, 0, 0, 0); | ||
2837 | d.BodySetLinearVel(Body, 0, 0, 0); | ||
2838 | disableBodySoft(); | ||
2839 | |||
2840 | // tell framework to fix it | ||
2841 | if (_parent == null) | ||
2842 | base.RequestPhysicsterseUpdate(); | ||
2843 | return; | ||
2844 | } | ||
2845 | |||
2846 | |||
2847 | //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)); | ||
2848 | //Console.WriteLine("Adiff " + Name + " = " + Adiff); | ||
2849 | if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02) | ||
2850 | && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02) | ||
2851 | && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02) | ||
2852 | // && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01)) | ||
2853 | && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large | ||
2854 | { | ||
2855 | _zeroFlag = true; | ||
2856 | //Console.WriteLine("ZFT 2"); | ||
2857 | m_throttleUpdates = false; | ||
2858 | } | ||
2859 | else | ||
2860 | { | ||
2861 | //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString()); | ||
2862 | _zeroFlag = false; | ||
2863 | m_lastUpdateSent = false; | ||
2864 | //m_throttleUpdates = false; | ||
2865 | } | ||
2866 | |||
2867 | if (_zeroFlag) | ||
2868 | { | ||
2869 | _velocity.X = 0.0f; | ||
2870 | _velocity.Y = 0.0f; | ||
2871 | _velocity.Z = 0.0f; | ||
2872 | |||
2873 | _acceleration.X = 0; | ||
2874 | _acceleration.Y = 0; | ||
2875 | _acceleration.Z = 0; | ||
2876 | |||
2877 | //_orientation.w = 0f; | ||
2878 | //_orientation.X = 0f; | ||
2879 | //_orientation.Y = 0f; | ||
2880 | //_orientation.Z = 0f; | ||
2881 | m_rotationalVelocity.X = 0; | ||
2882 | m_rotationalVelocity.Y = 0; | ||
2883 | m_rotationalVelocity.Z = 0; | ||
2884 | if (!m_lastUpdateSent) | ||
2885 | { | ||
2886 | m_throttleUpdates = false; | ||
2887 | throttleCounter = 0; | ||
2888 | m_rotationalVelocity = pv; | ||
2889 | |||
2890 | if (_parent == null) | ||
2891 | { | ||
2892 | base.RequestPhysicsterseUpdate(); | ||
2893 | } | ||
2894 | |||
2895 | m_lastUpdateSent = true; | ||
2896 | } | ||
2897 | } | ||
2898 | else | ||
2899 | { | ||
2900 | if (lastZeroFlag != _zeroFlag) | ||
2901 | { | ||
2902 | if (_parent == null) | ||
2903 | { | ||
2904 | base.RequestPhysicsterseUpdate(); | ||
2905 | } | ||
2906 | } | ||
2907 | |||
2908 | m_lastVelocity = _velocity; | ||
2909 | |||
2910 | _position = l_position; | ||
2911 | |||
2912 | _velocity.X = vel.X; | ||
2913 | _velocity.Y = vel.Y; | ||
2914 | _velocity.Z = vel.Z; | ||
2915 | |||
2916 | _acceleration = ((_velocity - m_lastVelocity) / 0.1f); | ||
2917 | _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f); | ||
2918 | //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString()); | ||
2919 | |||
2920 | // Note here that linearvelocity is affecting angular velocity... so I'm guessing this is a vehicle specific thing... | ||
2921 | // it does make sense to do this for tiny little instabilities with physical prim, however 0.5m/frame is fairly large. | ||
2922 | // reducing this to 0.02m/frame seems to help the angular rubberbanding quite a bit, however, to make sure it doesn't affect elevators and vehicles | ||
2923 | // adding these logical exclusion situations to maintain this where I think it was intended to be. | ||
2924 | if (m_throttleUpdates || PIDActive || (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) || (Amotor != IntPtr.Zero)) | ||
2925 | { | ||
2926 | m_minvelocity = 0.5f; | ||
2927 | } | ||
2928 | else | ||
2929 | { | ||
2930 | m_minvelocity = 0.02f; | ||
2931 | } | ||
2932 | |||
2933 | if (_velocity.ApproxEquals(pv, m_minvelocity)) | ||
2934 | { | ||
2935 | m_rotationalVelocity = pv; | ||
2936 | } | ||
2937 | else | ||
2938 | { | ||
2939 | m_rotationalVelocity = new Vector3(rotvel.X, rotvel.Y, rotvel.Z); | ||
2940 | } | ||
2941 | |||
2942 | //m_log.Debug("ODE: " + m_rotationalVelocity.ToString()); | ||
2943 | _orientation.X = ori.X; | ||
2944 | _orientation.Y = ori.Y; | ||
2945 | _orientation.Z = ori.Z; | ||
2946 | _orientation.W = ori.W; | ||
2947 | m_lastUpdateSent = false; | ||
2948 | if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) | ||
2949 | { | ||
2950 | if (_parent == null) | ||
2951 | { | ||
2952 | base.RequestPhysicsterseUpdate(); | ||
2953 | } | ||
2954 | } | ||
2955 | else | ||
2956 | { | ||
2957 | throttleCounter++; | ||
2958 | } | ||
2959 | } | ||
2960 | m_lastposition = l_position; | ||
2961 | } | ||
2962 | else | ||
2963 | { | ||
2964 | // Not a body.. so Make sure the client isn't interpolating | ||
2965 | _velocity.X = 0; | ||
2966 | _velocity.Y = 0; | ||
2967 | _velocity.Z = 0; | ||
2968 | |||
2969 | _acceleration.X = 0; | ||
2970 | _acceleration.Y = 0; | ||
2971 | _acceleration.Z = 0; | ||
2972 | |||
2973 | m_rotationalVelocity.X = 0; | ||
2974 | m_rotationalVelocity.Y = 0; | ||
2975 | m_rotationalVelocity.Z = 0; | ||
2976 | _zeroFlag = true; | ||
2977 | } | ||
2978 | } | ||
2979 | } | ||
2980 | |||
2981 | public override bool FloatOnWater | ||
2982 | { | ||
2983 | set { | ||
2984 | m_taintCollidesWater = value; | ||
2985 | _parent_scene.AddPhysicsActorTaint(this); | ||
2986 | } | ||
2987 | } | ||
2988 | |||
2989 | public override void SetMomentum(Vector3 momentum) | ||
2990 | { | ||
2991 | } | ||
2992 | |||
2993 | public override Vector3 PIDTarget | ||
2994 | { | ||
2995 | set | ||
2996 | { | ||
2997 | if (value.IsFinite()) | ||
2998 | { | ||
2999 | m_PIDTarget = value; | ||
3000 | } | ||
3001 | else | ||
3002 | m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name); | ||
3003 | } | ||
3004 | } | ||
3005 | |||
3006 | public override bool PIDActive { get; set; } | ||
3007 | public override float PIDTau { set { m_PIDTau = value; } } | ||
3008 | |||
3009 | public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } | ||
3010 | public override bool PIDHoverActive { set { m_useHoverPID = value; } } | ||
3011 | public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } | ||
3012 | public override float PIDHoverTau { set { m_PIDHoverTau = value; } } | ||
3013 | |||
3014 | public override Quaternion APIDTarget{ set { return; } } | ||
3015 | |||
3016 | public override bool APIDActive{ set { return; } } | ||
3017 | |||
3018 | public override float APIDStrength{ set { return; } } | ||
3019 | |||
3020 | public override float APIDDamping{ set { return; } } | ||
3021 | |||
3022 | private void createAMotor(Vector3 axis) | ||
3023 | { | ||
3024 | if (Body == IntPtr.Zero) | ||
3025 | return; | ||
3026 | |||
3027 | if (Amotor != IntPtr.Zero) | ||
3028 | { | ||
3029 | d.JointDestroy(Amotor); | ||
3030 | Amotor = IntPtr.Zero; | ||
3031 | } | ||
3032 | |||
3033 | float axisnum = 3; | ||
3034 | |||
3035 | axisnum = (axisnum - (axis.X + axis.Y + axis.Z)); | ||
3036 | |||
3037 | // PhysicsVector totalSize = new PhysicsVector(_size.X, _size.Y, _size.Z); | ||
3038 | |||
3039 | |||
3040 | // Inverse Inertia Matrix, set the X, Y, and/r Z inertia to 0 then invert it again. | ||
3041 | d.Mass objMass; | ||
3042 | d.MassSetZero(out objMass); | ||
3043 | DMassCopy(ref pMass, ref objMass); | ||
3044 | |||
3045 | //m_log.DebugFormat("1-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22); | ||
3046 | |||
3047 | Matrix4 dMassMat = FromDMass(objMass); | ||
3048 | |||
3049 | Matrix4 mathmat = Inverse(dMassMat); | ||
3050 | |||
3051 | /* | ||
3052 | //m_log.DebugFormat("2-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", mathmat[0, 0], mathmat[0, 1], mathmat[0, 2], mathmat[1, 0], mathmat[1, 1], mathmat[1, 2], mathmat[2, 0], mathmat[2, 1], mathmat[2, 2]); | ||
3053 | |||
3054 | mathmat = Inverse(mathmat); | ||
3055 | |||
3056 | |||
3057 | objMass = FromMatrix4(mathmat, ref objMass); | ||
3058 | //m_log.DebugFormat("3-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22); | ||
3059 | |||
3060 | mathmat = Inverse(mathmat); | ||
3061 | */ | ||
3062 | if (axis.X == 0) | ||
3063 | { | ||
3064 | mathmat.M33 = 50.0000001f; | ||
3065 | //objMass.I.M22 = 0; | ||
3066 | } | ||
3067 | if (axis.Y == 0) | ||
3068 | { | ||
3069 | mathmat.M22 = 50.0000001f; | ||
3070 | //objMass.I.M11 = 0; | ||
3071 | } | ||
3072 | if (axis.Z == 0) | ||
3073 | { | ||
3074 | mathmat.M11 = 50.0000001f; | ||
3075 | //objMass.I.M00 = 0; | ||
3076 | } | ||
3077 | |||
3078 | |||
3079 | |||
3080 | mathmat = Inverse(mathmat); | ||
3081 | objMass = FromMatrix4(mathmat, ref objMass); | ||
3082 | //m_log.DebugFormat("4-{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, ", objMass.I.M00, objMass.I.M01, objMass.I.M02, objMass.I.M10, objMass.I.M11, objMass.I.M12, objMass.I.M20, objMass.I.M21, objMass.I.M22); | ||
3083 | |||
3084 | //return; | ||
3085 | if (d.MassCheck(ref objMass)) | ||
3086 | { | ||
3087 | d.BodySetMass(Body, ref objMass); | ||
3088 | } | ||
3089 | else | ||
3090 | { | ||
3091 | //m_log.Debug("[PHYSICS]: Mass invalid, ignoring"); | ||
3092 | } | ||
3093 | |||
3094 | if (axisnum <= 0) | ||
3095 | return; | ||
3096 | // int dAMotorEuler = 1; | ||
3097 | |||
3098 | Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); | ||
3099 | d.JointAttach(Amotor, Body, IntPtr.Zero); | ||
3100 | d.JointSetAMotorMode(Amotor, 0); | ||
3101 | |||
3102 | d.JointSetAMotorNumAxes(Amotor,(int)axisnum); | ||
3103 | int i = 0; | ||
3104 | |||
3105 | if (axis.X == 0) | ||
3106 | { | ||
3107 | d.JointSetAMotorAxis(Amotor, i, 0, 1, 0, 0); | ||
3108 | i++; | ||
3109 | } | ||
3110 | |||
3111 | if (axis.Y == 0) | ||
3112 | { | ||
3113 | d.JointSetAMotorAxis(Amotor, i, 0, 0, 1, 0); | ||
3114 | i++; | ||
3115 | } | ||
3116 | |||
3117 | if (axis.Z == 0) | ||
3118 | { | ||
3119 | d.JointSetAMotorAxis(Amotor, i, 0, 0, 0, 1); | ||
3120 | i++; | ||
3121 | } | ||
3122 | |||
3123 | for (int j = 0; j < (int)axisnum; j++) | ||
3124 | { | ||
3125 | //d.JointSetAMotorAngle(Amotor, j, 0); | ||
3126 | } | ||
3127 | |||
3128 | //d.JointSetAMotorAngle(Amotor, 1, 0); | ||
3129 | //d.JointSetAMotorAngle(Amotor, 2, 0); | ||
3130 | |||
3131 | // These lowstops and high stops are effectively (no wiggle room) | ||
3132 | d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0f); | ||
3133 | d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f); | ||
3134 | d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0f); | ||
3135 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f); | ||
3136 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f); | ||
3137 | d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f); | ||
3138 | //d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 9000f); | ||
3139 | d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f); | ||
3140 | d.JointSetAMotorParam(Amotor, (int)dParam.FMax, Mass * 50f);// | ||
3141 | } | ||
3142 | |||
3143 | private Matrix4 FromDMass(d.Mass pMass) | ||
3144 | { | ||
3145 | Matrix4 obj; | ||
3146 | obj.M11 = pMass.I.M00; | ||
3147 | obj.M12 = pMass.I.M01; | ||
3148 | obj.M13 = pMass.I.M02; | ||
3149 | obj.M14 = 0; | ||
3150 | obj.M21 = pMass.I.M10; | ||
3151 | obj.M22 = pMass.I.M11; | ||
3152 | obj.M23 = pMass.I.M12; | ||
3153 | obj.M24 = 0; | ||
3154 | obj.M31 = pMass.I.M20; | ||
3155 | obj.M32 = pMass.I.M21; | ||
3156 | obj.M33 = pMass.I.M22; | ||
3157 | obj.M34 = 0; | ||
3158 | obj.M41 = 0; | ||
3159 | obj.M42 = 0; | ||
3160 | obj.M43 = 0; | ||
3161 | obj.M44 = 1; | ||
3162 | return obj; | ||
3163 | } | ||
3164 | |||
3165 | private d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj) | ||
3166 | { | ||
3167 | obj.I.M00 = pMat[0, 0]; | ||
3168 | obj.I.M01 = pMat[0, 1]; | ||
3169 | obj.I.M02 = pMat[0, 2]; | ||
3170 | obj.I.M10 = pMat[1, 0]; | ||
3171 | obj.I.M11 = pMat[1, 1]; | ||
3172 | obj.I.M12 = pMat[1, 2]; | ||
3173 | obj.I.M20 = pMat[2, 0]; | ||
3174 | obj.I.M21 = pMat[2, 1]; | ||
3175 | obj.I.M22 = pMat[2, 2]; | ||
3176 | return obj; | ||
3177 | } | ||
3178 | |||
3179 | public override void SubscribeEvents(int ms) | ||
3180 | { | ||
3181 | m_eventsubscription = ms; | ||
3182 | _parent_scene.AddCollisionEventReporting(this); | ||
3183 | } | ||
3184 | |||
3185 | public override void UnSubscribeEvents() | ||
3186 | { | ||
3187 | _parent_scene.RemoveCollisionEventReporting(this); | ||
3188 | m_eventsubscription = 0; | ||
3189 | } | ||
3190 | |||
3191 | public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) | ||
3192 | { | ||
3193 | CollisionEventsThisFrame.AddCollider(CollidedWith, contact); | ||
3194 | } | ||
3195 | |||
3196 | public void SendCollisions() | ||
3197 | { | ||
3198 | if (m_collisionsOnPreviousFrame || CollisionEventsThisFrame.Count > 0) | ||
3199 | { | ||
3200 | base.SendCollisionUpdate(CollisionEventsThisFrame); | ||
3201 | |||
3202 | if (CollisionEventsThisFrame.Count > 0) | ||
3203 | { | ||
3204 | m_collisionsOnPreviousFrame = true; | ||
3205 | CollisionEventsThisFrame.Clear(); | ||
3206 | } | ||
3207 | else | ||
3208 | { | ||
3209 | m_collisionsOnPreviousFrame = false; | ||
3210 | } | ||
3211 | } | ||
3212 | } | ||
3213 | |||
3214 | public override bool SubscribedEvents() | ||
3215 | { | ||
3216 | if (m_eventsubscription > 0) | ||
3217 | return true; | ||
3218 | return false; | ||
3219 | } | ||
3220 | |||
3221 | public static Matrix4 Inverse(Matrix4 pMat) | ||
3222 | { | ||
3223 | if (determinant3x3(pMat) == 0) | ||
3224 | { | ||
3225 | return Matrix4.Identity; // should probably throw an error. singluar matrix inverse not possible | ||
3226 | } | ||
3227 | |||
3228 | return (Adjoint(pMat) / determinant3x3(pMat)); | ||
3229 | } | ||
3230 | |||
3231 | public static Matrix4 Adjoint(Matrix4 pMat) | ||
3232 | { | ||
3233 | Matrix4 adjointMatrix = new Matrix4(); | ||
3234 | for (int i=0; i<4; i++) | ||
3235 | { | ||
3236 | for (int j=0; j<4; j++) | ||
3237 | { | ||
3238 | Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j))))); | ||
3239 | } | ||
3240 | } | ||
3241 | |||
3242 | adjointMatrix = Transpose(adjointMatrix); | ||
3243 | return adjointMatrix; | ||
3244 | } | ||
3245 | |||
3246 | public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol) | ||
3247 | { | ||
3248 | Matrix4 minor = new Matrix4(); | ||
3249 | int m = 0, n = 0; | ||
3250 | for (int i = 0; i < 4; i++) | ||
3251 | { | ||
3252 | if (i == iRow) | ||
3253 | continue; | ||
3254 | n = 0; | ||
3255 | for (int j = 0; j < 4; j++) | ||
3256 | { | ||
3257 | if (j == iCol) | ||
3258 | continue; | ||
3259 | Matrix4SetValue(ref minor, m,n, matrix[i, j]); | ||
3260 | n++; | ||
3261 | } | ||
3262 | m++; | ||
3263 | } | ||
3264 | |||
3265 | return minor; | ||
3266 | } | ||
3267 | |||
3268 | public static Matrix4 Transpose(Matrix4 pMat) | ||
3269 | { | ||
3270 | Matrix4 transposeMatrix = new Matrix4(); | ||
3271 | for (int i = 0; i < 4; i++) | ||
3272 | for (int j = 0; j < 4; j++) | ||
3273 | Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]); | ||
3274 | return transposeMatrix; | ||
3275 | } | ||
3276 | |||
3277 | public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val) | ||
3278 | { | ||
3279 | switch (r) | ||
3280 | { | ||
3281 | case 0: | ||
3282 | switch (c) | ||
3283 | { | ||
3284 | case 0: | ||
3285 | pMat.M11 = val; | ||
3286 | break; | ||
3287 | case 1: | ||
3288 | pMat.M12 = val; | ||
3289 | break; | ||
3290 | case 2: | ||
3291 | pMat.M13 = val; | ||
3292 | break; | ||
3293 | case 3: | ||
3294 | pMat.M14 = val; | ||
3295 | break; | ||
3296 | } | ||
3297 | |||
3298 | break; | ||
3299 | case 1: | ||
3300 | switch (c) | ||
3301 | { | ||
3302 | case 0: | ||
3303 | pMat.M21 = val; | ||
3304 | break; | ||
3305 | case 1: | ||
3306 | pMat.M22 = val; | ||
3307 | break; | ||
3308 | case 2: | ||
3309 | pMat.M23 = val; | ||
3310 | break; | ||
3311 | case 3: | ||
3312 | pMat.M24 = val; | ||
3313 | break; | ||
3314 | } | ||
3315 | |||
3316 | break; | ||
3317 | case 2: | ||
3318 | switch (c) | ||
3319 | { | ||
3320 | case 0: | ||
3321 | pMat.M31 = val; | ||
3322 | break; | ||
3323 | case 1: | ||
3324 | pMat.M32 = val; | ||
3325 | break; | ||
3326 | case 2: | ||
3327 | pMat.M33 = val; | ||
3328 | break; | ||
3329 | case 3: | ||
3330 | pMat.M34 = val; | ||
3331 | break; | ||
3332 | } | ||
3333 | |||
3334 | break; | ||
3335 | case 3: | ||
3336 | switch (c) | ||
3337 | { | ||
3338 | case 0: | ||
3339 | pMat.M41 = val; | ||
3340 | break; | ||
3341 | case 1: | ||
3342 | pMat.M42 = val; | ||
3343 | break; | ||
3344 | case 2: | ||
3345 | pMat.M43 = val; | ||
3346 | break; | ||
3347 | case 3: | ||
3348 | pMat.M44 = val; | ||
3349 | break; | ||
3350 | } | ||
3351 | |||
3352 | break; | ||
3353 | } | ||
3354 | } | ||
3355 | |||
3356 | private static float determinant3x3(Matrix4 pMat) | ||
3357 | { | ||
3358 | float det = 0; | ||
3359 | float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2]; | ||
3360 | float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0]; | ||
3361 | float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1]; | ||
3362 | float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2]; | ||
3363 | float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0]; | ||
3364 | float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1]; | ||
3365 | |||
3366 | det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6); | ||
3367 | return det; | ||
3368 | } | ||
3369 | |||
3370 | private static void DMassCopy(ref d.Mass src, ref d.Mass dst) | ||
3371 | { | ||
3372 | dst.c.W = src.c.W; | ||
3373 | dst.c.X = src.c.X; | ||
3374 | dst.c.Y = src.c.Y; | ||
3375 | dst.c.Z = src.c.Z; | ||
3376 | dst.mass = src.mass; | ||
3377 | dst.I.M00 = src.I.M00; | ||
3378 | dst.I.M01 = src.I.M01; | ||
3379 | dst.I.M02 = src.I.M02; | ||
3380 | dst.I.M10 = src.I.M10; | ||
3381 | dst.I.M11 = src.I.M11; | ||
3382 | dst.I.M12 = src.I.M12; | ||
3383 | dst.I.M20 = src.I.M20; | ||
3384 | dst.I.M21 = src.I.M21; | ||
3385 | dst.I.M22 = src.I.M22; | ||
3386 | } | ||
3387 | |||
3388 | public override void SetMaterial(int pMaterial) | ||
3389 | { | ||
3390 | m_material = pMaterial; | ||
3391 | } | ||
3392 | |||
3393 | private void CheckMeshAsset() | ||
3394 | { | ||
3395 | if (_pbs.SculptEntry && !m_assetFailed && _pbs.SculptTexture != UUID.Zero) | ||
3396 | { | ||
3397 | m_assetFailed = true; | ||
3398 | Util.FireAndForget(delegate | ||
3399 | { | ||
3400 | RequestAssetDelegate assetProvider = _parent_scene.RequestAssetMethod; | ||
3401 | if (assetProvider != null) | ||
3402 | assetProvider(_pbs.SculptTexture, MeshAssetReceived); | ||
3403 | }, null, "ODEPrim.CheckMeshAsset"); | ||
3404 | } | ||
3405 | } | ||
3406 | |||
3407 | private void MeshAssetReceived(AssetBase asset) | ||
3408 | { | ||
3409 | if (asset != null && asset.Data != null && asset.Data.Length > 0) | ||
3410 | { | ||
3411 | if (!_pbs.SculptEntry) | ||
3412 | return; | ||
3413 | if (_pbs.SculptTexture.ToString() != asset.ID) | ||
3414 | return; | ||
3415 | |||
3416 | _pbs.SculptData = new byte[asset.Data.Length]; | ||
3417 | asset.Data.CopyTo(_pbs.SculptData, 0); | ||
3418 | // m_assetFailed = false; | ||
3419 | |||
3420 | // m_log.DebugFormat( | ||
3421 | // "[ODE PRIM]: Received mesh/sculpt data asset {0} with {1} bytes for {2} at {3} in {4}", | ||
3422 | // _pbs.SculptTexture, _pbs.SculptData.Length, Name, _position, _parent_scene.Name); | ||
3423 | |||
3424 | m_taintshape = true; | ||
3425 | _parent_scene.AddPhysicsActorTaint(this); | ||
3426 | } | ||
3427 | else | ||
3428 | { | ||
3429 | m_log.WarnFormat( | ||
3430 | "[ODE PRIM]: Could not get mesh/sculpt asset {0} for {1} at {2} in {3}", | ||
3431 | _pbs.SculptTexture, Name, _position, _parent_scene.PhysicsSceneName); | ||
3432 | } | ||
3433 | } | ||
3434 | } | ||
3435 | } \ No newline at end of file | ||