aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/UbitOdePlugin
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics/UbitOdePlugin')
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs5345
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs5155
2 files changed, 4763 insertions, 5737 deletions
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
index eaf0d0a..f083d38 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
@@ -25,6 +25,11 @@
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */ 26 */
27 27
28/* Revision 2011/12 by Ubit Umarov
29 *
30 *
31 */
32
28/* 33/*
29 * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces 34 * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces
30 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: 35 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
@@ -48,250 +53,251 @@ using System.Runtime.InteropServices;
48using System.Threading; 53using System.Threading;
49using log4net; 54using log4net;
50using OpenMetaverse; 55using OpenMetaverse;
51using Ode.NET; 56using OdeAPI;
52using OpenSim.Framework; 57using OpenSim.Framework;
53using OpenSim.Region.Physics.Manager; 58using OpenSim.Region.Physics.Manager;
54 59
55namespace OpenSim.Region.Physics.OdePlugin 60namespace OpenSim.Region.Physics.OdePlugin
56{ 61{
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 62 public class OdePrim : PhysicsActor
61 { 63 {
62 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); 64 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
63 65
64 private bool m_isphysical; 66 private bool m_isphysical;
67 private bool m_fakeisphysical;
68 private bool m_isphantom;
69 private bool m_fakeisphantom;
70 internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
71 private bool m_fakeisVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
65 72
66 public int ExpectedCollisionContacts { get { return m_expectedCollisionContacts; } } 73 protected bool m_building;
67 private int m_expectedCollisionContacts = 0; 74 protected bool m_forcePosOrRotation;
75 private bool m_iscolliding;
68 76
69 /// <summary> 77 internal bool m_isSelected;
70 /// Is this prim subject to physics? Even if not, it's still solid for collision purposes. 78 private bool m_delaySelect;
71 /// </summary> 79 private bool m_lastdoneSelected;
72 public override bool IsPhysical 80 internal bool m_outbounds;
73 { 81
74 get { return m_isphysical; } 82 private Quaternion m_lastorientation;
75 set 83 private Quaternion _orientation;
76 {
77 m_isphysical = value;
78 if (!m_isphysical) // Zero the remembered last velocity
79 m_lastVelocity = Vector3.Zero;
80 }
81 }
82 84
83 private Vector3 _position; 85 private Vector3 _position;
84 private Vector3 _velocity; 86 private Vector3 _velocity;
85 private Vector3 _torque; 87 private Vector3 _torque;
86 private Vector3 m_lastVelocity; 88 private Vector3 m_lastVelocity;
87 private Vector3 m_lastposition; 89 private Vector3 m_lastposition;
88 private Quaternion m_lastorientation = new Quaternion();
89 private Vector3 m_rotationalVelocity; 90 private Vector3 m_rotationalVelocity;
90 private Vector3 _size; 91 private Vector3 _size;
91 private Vector3 _acceleration; 92 private Vector3 _acceleration;
92 // private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f);
93 private Quaternion _orientation;
94 private Vector3 m_taintposition;
95 private Vector3 m_taintsize;
96 private Vector3 m_taintVelocity;
97 private Vector3 m_taintTorque;
98 private Quaternion m_taintrot;
99 private Vector3 m_angularlock = Vector3.One; 93 private Vector3 m_angularlock = Vector3.One;
100 private Vector3 m_taintAngularLock = Vector3.One; 94 private IntPtr Amotor;
101 private IntPtr Amotor = IntPtr.Zero;
102 95
103 private object m_assetsLock = new object(); 96 private Vector3 m_force;
104 private bool m_assetFailed = false; 97 private Vector3 m_forceacc;
98 private Vector3 m_angularForceacc;
99
100 private float m_invTimeStep;
101 private float m_timeStep;
105 102
106 private Vector3 m_PIDTarget; 103 private Vector3 m_PIDTarget;
107 private float m_PIDTau; 104 private float m_PIDTau;
108 private float PID_D = 35f;
109 private float PID_G = 25f;
110 private bool m_usePID; 105 private bool m_usePID;
111 106
112 // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
113 // and are for non-VEHICLES only.
114
115 private float m_PIDHoverHeight; 107 private float m_PIDHoverHeight;
116 private float m_PIDHoverTau; 108 private float m_PIDHoverTau;
117 private bool m_useHoverPID; 109 private bool m_useHoverPID;
118 private PIDHoverType m_PIDHoverType = PIDHoverType.Ground; 110 private PIDHoverType m_PIDHoverType;
119 private float m_targetHoverHeight; 111 private float m_targetHoverHeight;
120 private float m_groundHeight; 112 private float m_groundHeight;
121 private float m_waterHeight; 113 private float m_waterHeight;
122 private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. 114 private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
123 115
124 // private float m_tensor = 5f; 116 private int body_autodisable_frames;
125 private int body_autodisable_frames = 20; 117 public int bodydisablecontrol;
126
127 118
128 private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom
129 | CollisionCategories.Space
130 | CollisionCategories.Body
131 | CollisionCategories.Character
132 );
133 private bool m_taintshape;
134 private bool m_taintPhysics;
135 private bool m_collidesLand = true;
136 private bool m_collidesWater;
137 119
138 // Default we're a Geometry 120 // Default we're a Geometry
139 private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); 121 private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
122 // Default colide nonphysical don't try to colide with anything
123 private const CollisionCategories m_default_collisionFlagsNotPhysical = 0;
124
125 private const CollisionCategories m_default_collisionFlagsPhysical = (CollisionCategories.Geom |
126 CollisionCategories.Character |
127 CollisionCategories.Land |
128 CollisionCategories.VolumeDtc);
129
130// private bool m_collidesLand = true;
131 private bool m_collidesWater;
132// public bool m_returnCollisions;
133
134 private bool m_NoColide; // for now only for internal use for bad meshs
135
140 136
141 // Default, Collide with Other Geometries, spaces and Bodies 137 // Default, Collide with Other Geometries, spaces and Bodies
142 private CollisionCategories m_collisionFlags = m_default_collisionFlags; 138 private CollisionCategories m_collisionFlags = m_default_collisionFlagsNotPhysical;
143 139
144 public bool m_taintremove { get; private set; } 140 public bool m_disabled;
145 public bool m_taintdisable { get; private set; }
146 internal bool m_disabled;
147 public bool m_taintadd { get; private set; }
148 public bool m_taintselected { get; private set; }
149 public bool m_taintCollidesWater { get; private set; }
150 141
151 private bool m_taintforce = false; 142 private uint m_localID;
152 private bool m_taintaddangularforce = false;
153 private Vector3 m_force;
154 private List<Vector3> m_forcelist = new List<Vector3>();
155 private List<Vector3> m_angularforcelist = new List<Vector3>();
156 143
144 private IMesh m_mesh;
145 private object m_meshlock = new object();
157 private PrimitiveBaseShape _pbs; 146 private PrimitiveBaseShape _pbs;
158 private OdeScene _parent_scene;
159 147
160 /// <summary> 148 private UUID? m_assetID;
161 /// The physics space which contains prim geometries 149 private MeshState m_meshState;
162 /// </summary> 150
163 public IntPtr m_targetSpace = IntPtr.Zero; 151 public OdeScene _parent_scene;
164 152
165 /// <summary> 153 /// <summary>
166 /// The prim geometry, used for collision detection. 154 /// The physics space which contains prim geometry
167 /// </summary> 155 /// </summary>
168 /// <remarks> 156 public IntPtr m_targetSpace;
169 /// This is never null except for a brief period when the geometry needs to be replaced (due to resizing or
170 /// mesh change) or when the physical prim is being removed from the scene.
171 /// </remarks>
172 public IntPtr prim_geom { get; private set; }
173 157
174 public IntPtr _triMeshData { get; private set; } 158 public IntPtr prim_geom;
159 public IntPtr _triMeshData;
175 160
176 private IntPtr _linkJointGroup = IntPtr.Zero;
177 private PhysicsActor _parent; 161 private PhysicsActor _parent;
178 private PhysicsActor m_taintparent;
179 162
180 private List<OdePrim> childrenPrim = new List<OdePrim>(); 163 private List<OdePrim> childrenPrim = new List<OdePrim>();
181 164
182 private bool iscolliding; 165 public float m_collisionscore;
183 private bool m_isSelected; 166 private int m_colliderfilter = 0;
184
185 internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
186
187 private bool m_throttleUpdates;
188 private int throttleCounter;
189 public int m_interpenetrationcount { get; private set; }
190 internal float m_collisionscore;
191 public int m_roundsUnderMotionThreshold { get; private set; }
192 private int m_crossingfailures;
193 167
194 public bool outofBounds { get; private set; } 168 public IntPtr collide_geom; // for objects: geom if single prim space it linkset
195 private float m_density = 10.000006836f; // Aluminum g/cm3;
196 169
197 public bool _zeroFlag { get; private set; } 170 private float m_density;
171 private byte m_shapetype;
172 public bool _zeroFlag;
198 private bool m_lastUpdateSent; 173 private bool m_lastUpdateSent;
199 174
200 public IntPtr Body = IntPtr.Zero; 175 public IntPtr Body;
176
201 private Vector3 _target_velocity; 177 private Vector3 _target_velocity;
202 private d.Mass pMass;
203 178
204 private int m_eventsubscription; 179 public Vector3 m_OBBOffset;
205 private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate(); 180 public Vector3 m_OBB;
181 public float primOOBradiusSQ;
206 182
207 /// <summary> 183 private bool m_hasOBB = true;
208 /// Signal whether there were collisions on the previous frame, so we know if we need to send the 184
209 /// empty CollisionEventsThisFrame to the prim so that it can detect the end of a collision. 185 private float m_physCost;
210 /// </summary> 186 private float m_streamCost;
211 /// <remarks> 187
212 /// This is probably a temporary measure, pending storing this information consistently in CollisionEventUpdate itself. 188 public d.Mass primdMass; // prim inertia information on it's own referencial
213 /// </remarks> 189 float primMass; // prim own mass
214 private bool m_collisionsOnPreviousFrame; 190 float primVolume; // prim own volume;
191 float _mass; // object mass acording to case
192
193 public int givefakepos;
194 private Vector3 fakepos;
195 public int givefakeori;
196 private Quaternion fakeori;
215 197
216 private IntPtr m_linkJoint = IntPtr.Zero; 198 private int m_eventsubscription;
199 private int m_cureventsubscription;
200 private CollisionEventUpdate CollisionEventsThisFrame = null;
201 private bool SentEmptyCollisionsEvent;
217 202
218 internal volatile bool childPrim; 203 public volatile bool childPrim;
219 204
220 private ODEDynamics m_vehicle; 205 public ODEDynamics m_vehicle;
221 206
222 internal int m_material = (int)Material.Wood; 207 internal int m_material = (int)Material.Wood;
208 private float mu;
209 private float bounce;
223 210
224 public OdePrim( 211 /// <summary>
225 String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, 212 /// Is this prim subject to physics? Even if not, it's still solid for collision purposes.
226 Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical) 213 /// </summary>
214 public override bool IsPhysical // this is not reliable for internal use
227 { 215 {
228 Name = primName; 216 get { return m_fakeisphysical; }
229 m_vehicle = new ODEDynamics(); 217 set
230 //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
231
232 if (!pos.IsFinite())
233 { 218 {
234 pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), 219 m_fakeisphysical = value; // we show imediatly to outside that we changed physical
235 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); 220 // and also to stop imediatly some updates
236 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name); 221 // but real change will only happen in taintprocessing
237 }
238 _position = pos;
239 m_taintposition = pos;
240 PID_D = parent_scene.bodyPIDD;
241 PID_G = parent_scene.bodyPIDG;
242 m_density = parent_scene.geomDefaultDensity;
243 // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
244 body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
245 222
246 prim_geom = IntPtr.Zero; 223 if (!value) // Zero the remembered last velocity
224 m_lastVelocity = Vector3.Zero;
225 AddChange(changes.Physical, value);
226 }
227 }
247 228
248 if (!pos.IsFinite()) 229 public override bool IsVolumeDtc
230 {
231 get { return m_fakeisVolumeDetect; }
232 set
249 { 233 {
250 size = new Vector3(0.5f, 0.5f, 0.5f); 234 m_fakeisVolumeDetect = value;
251 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name); 235 AddChange(changes.VolumeDtc, value);
252 } 236 }
237 }
253 238
254 if (size.X <= 0) size.X = 0.01f; 239 public override bool Phantom // this is not reliable for internal use
255 if (size.Y <= 0) size.Y = 0.01f; 240 {
256 if (size.Z <= 0) size.Z = 0.01f; 241 get { return m_fakeisphantom; }
257 242 set
258 _size = size; 243 {
259 m_taintsize = _size; 244 m_fakeisphantom = value;
245 AddChange(changes.Phantom, value);
246 }
247 }
260 248
261 if (!QuaternionIsFinite(rotation)) 249 public override bool Building // this is not reliable for internal use
250 {
251 get { return m_building; }
252 set
262 { 253 {
263 rotation = Quaternion.Identity; 254 if (value)
264 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name); 255 m_building = true;
256 AddChange(changes.building, value);
265 } 257 }
258 }
266 259
267 _orientation = rotation; 260 public override void getContactData(ref ContactData cdata)
268 m_taintrot = _orientation; 261 {
269 _pbs = pbs; 262 cdata.mu = mu;
263 cdata.bounce = bounce;
270 264
271 _parent_scene = parent_scene; 265 // cdata.softcolide = m_softcolide;
272 m_targetSpace = (IntPtr)0; 266 cdata.softcolide = false;
273 267
274 if (pos.Z < 0) 268 if (m_isphysical)
275 { 269 {
276 IsPhysical = false; 270 ODEDynamics veh;
271 if (_parent != null)
272 veh = ((OdePrim)_parent).m_vehicle;
273 else
274 veh = m_vehicle;
275
276 if (veh != null && veh.Type != Vehicle.TYPE_NONE)
277 cdata.mu *= veh.FrictionFactor;
278// cdata.mu *= 0;
277 } 279 }
278 else 280 }
281
282 public override float PhysicsCost
283 {
284 get
279 { 285 {
280 IsPhysical = pisPhysical; 286 return m_physCost;
281 // If we're physical, we need to be in the master space for now.
282 // linksets *should* be in a space together.. but are not currently
283 if (IsPhysical)
284 m_targetSpace = _parent_scene.space;
285 } 287 }
288 }
286 289
287 m_taintadd = true; 290 public override float StreamCost
288 m_assetFailed = false; 291 {
289 _parent_scene.AddPhysicsActorTaint(this); 292 get
293 {
294 return m_streamCost;
295 }
290 } 296 }
291 297
292 public override int PhysicsActorType 298 public override int PhysicsActorType
293 { 299 {
294 get { return (int) ActorTypes.Prim; } 300 get { return (int)ActorTypes.Prim; }
295 set { return; } 301 set { return; }
296 } 302 }
297 303
@@ -301,6 +307,23 @@ namespace OpenSim.Region.Physics.OdePlugin
301 set { return; } 307 set { return; }
302 } 308 }
303 309
310 public override uint LocalID
311 {
312 get { return m_localID; }
313 set { m_localID = value; }
314 }
315
316 public override PhysicsActor ParentActor
317 {
318 get
319 {
320 if (childPrim)
321 return _parent;
322 else
323 return (PhysicsActor)this;
324 }
325 }
326
304 public override bool Grabbed 327 public override bool Grabbed
305 { 328 {
306 set { return; } 329 set { return; }
@@ -310,2383 +333,3063 @@ namespace OpenSim.Region.Physics.OdePlugin
310 { 333 {
311 set 334 set
312 { 335 {
313 // This only makes the object not collidable if the object 336 if (value)
314 // is physical or the object is modified somehow *IN THE FUTURE* 337 m_isSelected = value; // if true set imediatly to stop moves etc
315 // without this, if an avatar selects prim, they can walk right 338 AddChange(changes.Selected, value);
316 // through it while it's selected 339 }
317 m_collisionscore = 0; 340 }
318 341
319 if ((IsPhysical && !_zeroFlag) || !value) 342 public override bool Flying
343 {
344 // no flying prims for you
345 get { return false; }
346 set { }
347 }
348
349 public override bool IsColliding
350 {
351 get { return m_iscolliding; }
352 set
353 {
354 if (value)
320 { 355 {
321 m_taintselected = value; 356 m_colliderfilter += 2;
322 _parent_scene.AddPhysicsActorTaint(this); 357 if (m_colliderfilter > 2)
358 m_colliderfilter = 2;
323 } 359 }
324 else 360 else
325 { 361 {
326 m_taintselected = value; 362 m_colliderfilter--;
327 m_isSelected = value; 363 if (m_colliderfilter < 0)
364 m_colliderfilter = 0;
328 } 365 }
329 366
330 if (m_isSelected) 367 if (m_colliderfilter == 0)
331 disableBodySoft(); 368 m_iscolliding = false;
369 else
370 m_iscolliding = true;
332 } 371 }
333 } 372 }
334 373
335 /// <summary> 374 public override bool CollidingGround
336 /// Set a new geometry for this prim.
337 /// </summary>
338 /// <param name="geom"></param>
339 private void SetGeom(IntPtr geom)
340 { 375 {
341 prim_geom = geom; 376 get { return false; }
342//Console.WriteLine("SetGeom to " + prim_geom + " for " + Name); 377 set { return; }
378 }
379
380 public override bool CollidingObj
381 {
382 get { return false; }
383 set { return; }
384 }
385
343 386
344 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 387 public override bool ThrottleUpdates {get;set;}
345 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 388
389 public override bool Stopped
390 {
391 get { return _zeroFlag; }
392 }
346 393
347 _parent_scene.geom_name_map[prim_geom] = Name; 394 public override Vector3 Position
348 _parent_scene.actor_name_map[prim_geom] = this; 395 {
396 get
397 {
398 if (givefakepos > 0)
399 return fakepos;
400 else
401 return _position;
402 }
349 403
350 if (childPrim) 404 set
351 { 405 {
352 if (_parent != null && _parent is OdePrim) 406 fakepos = value;
353 { 407 givefakepos++;
354 OdePrim parent = (OdePrim)_parent; 408 AddChange(changes.Position, value);
355//Console.WriteLine("SetGeom calls ChildSetGeom");
356 parent.ChildSetGeom(this);
357 }
358 } 409 }
359 //m_log.Warn("Setting Geom to: " + prim_geom);
360 } 410 }
361 411
362 private void enableBodySoft() 412 public override Vector3 Size
363 { 413 {
364 if (!childPrim) 414 get { return _size; }
415 set
365 { 416 {
366 if (IsPhysical && Body != IntPtr.Zero) 417 if (value.IsFinite())
367 { 418 {
368 d.BodyEnable(Body); 419 _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, value, m_shapetype);
369 if (m_vehicle.Type != Vehicle.TYPE_NONE) 420 }
370 m_vehicle.Enable(Body, _parent_scene); 421 else
422 {
423 m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name);
371 } 424 }
372
373 m_disabled = false;
374 } 425 }
375 } 426 }
376 427
377 private void disableBodySoft() 428 public override float Mass
378 { 429 {
379 m_disabled = true; 430 get { return primMass; }
380
381 if (IsPhysical && Body != IntPtr.Zero)
382 {
383 d.BodyDisable(Body);
384 }
385 } 431 }
386 432
387 /// <summary> 433 public override Vector3 Force
388 /// Make a prim subject to physics.
389 /// </summary>
390 private void enableBody()
391 { 434 {
392 // Don't enable this body if we're a child prim 435 get { return m_force; }
393 // this should be taken care of in the parent function not here 436 set
394 if (!childPrim)
395 { 437 {
396 // Sets the geom to a body 438 if (value.IsFinite())
397 Body = d.BodyCreate(_parent_scene.world);
398
399 setMass();
400 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
401 d.Quaternion myrot = new d.Quaternion();
402 myrot.X = _orientation.X;
403 myrot.Y = _orientation.Y;
404 myrot.Z = _orientation.Z;
405 myrot.W = _orientation.W;
406 d.BodySetQuaternion(Body, ref myrot);
407 d.GeomSetBody(prim_geom, Body);
408 m_collisionCategories |= CollisionCategories.Body;
409 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
410
411 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
412 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
413
414 d.BodySetAutoDisableFlag(Body, true);
415 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
416
417 // disconnect from world gravity so we can apply buoyancy
418 d.BodySetGravityMode (Body, false);
419
420 m_interpenetrationcount = 0;
421 m_collisionscore = 0;
422 m_disabled = false;
423
424 // The body doesn't already have a finite rotation mode set here
425 if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0.0f)) && _parent == null)
426 { 439 {
427 createAMotor(m_angularlock); 440 AddChange(changes.Force, value);
428 } 441 }
429 if (m_vehicle.Type != Vehicle.TYPE_NONE) 442 else
430 { 443 {
431 m_vehicle.Enable(Body, _parent_scene); 444 m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name);
432 } 445 }
433
434 _parent_scene.ActivatePrim(this);
435 } 446 }
436 } 447 }
437 448
438 #region Mass Calculation 449 public override void SetVolumeDetect(int param)
439
440 private float CalculateMass()
441 { 450 {
442 float volume = _size.X * _size.Y * _size.Z; // default 451 m_fakeisVolumeDetect = (param != 0);
443 float tmp; 452 AddChange(changes.VolumeDtc, m_fakeisVolumeDetect);
453 }
444 454
445 float returnMass = 0; 455 public override Vector3 GeometricCenter
446 float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; 456 {
447 float hollowVolume = hollowAmount * hollowAmount; 457 // this is not real geometric center but a average of positions relative to root prim acording to
448 458 // http://wiki.secondlife.com/wiki/llGetGeometricCenter
449 switch (_pbs.ProfileShape) 459 // ignoring tortured prims details since sl also seems to ignore
460 // so no real use in doing it on physics
461 get
450 { 462 {
451 case ProfileShape.Square: 463 return Vector3.Zero;
452 // default box 464 }
453 465 }
454 if (_pbs.PathCurve == (byte)Extrusion.Straight)
455 {
456 if (hollowAmount > 0.0)
457 {
458 switch (_pbs.HollowShape)
459 {
460 case HollowShape.Square:
461 case HollowShape.Same:
462 break;
463
464 case HollowShape.Circle:
465
466 hollowVolume *= 0.78539816339f;
467 break;
468
469 case HollowShape.Triangle:
470
471 hollowVolume *= (0.5f * .5f);
472 break;
473
474 default:
475 hollowVolume = 0;
476 break;
477 }
478 volume *= (1.0f - hollowVolume);
479 }
480 }
481
482 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
483 {
484 //a tube
485
486 volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX);
487 tmp= 1.0f -2.0e-2f * (float)(200 - _pbs.PathScaleY);
488 volume -= volume*tmp*tmp;
489
490 if (hollowAmount > 0.0)
491 {
492 hollowVolume *= hollowAmount;
493
494 switch (_pbs.HollowShape)
495 {
496 case HollowShape.Square:
497 case HollowShape.Same:
498 break;
499
500 case HollowShape.Circle:
501 hollowVolume *= 0.78539816339f;;
502 break;
503
504 case HollowShape.Triangle:
505 hollowVolume *= 0.5f * 0.5f;
506 break;
507 default:
508 hollowVolume = 0;
509 break;
510 }
511 volume *= (1.0f - hollowVolume);
512 }
513 }
514
515 break;
516
517 case ProfileShape.Circle:
518
519 if (_pbs.PathCurve == (byte)Extrusion.Straight)
520 {
521 volume *= 0.78539816339f; // elipse base
522
523 if (hollowAmount > 0.0)
524 {
525 switch (_pbs.HollowShape)
526 {
527 case HollowShape.Same:
528 case HollowShape.Circle:
529 break;
530
531 case HollowShape.Square:
532 hollowVolume *= 0.5f * 2.5984480504799f;
533 break;
534
535 case HollowShape.Triangle:
536 hollowVolume *= .5f * 1.27323954473516f;
537 break;
538
539 default:
540 hollowVolume = 0;
541 break;
542 }
543 volume *= (1.0f - hollowVolume);
544 }
545 }
546
547 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
548 {
549 volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX);
550 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
551 volume *= (1.0f - tmp * tmp);
552
553 if (hollowAmount > 0.0)
554 {
555
556 // calculate the hollow volume by it's shape compared to the prim shape
557 hollowVolume *= hollowAmount;
558
559 switch (_pbs.HollowShape)
560 {
561 case HollowShape.Same:
562 case HollowShape.Circle:
563 break;
564
565 case HollowShape.Square:
566 hollowVolume *= 0.5f * 2.5984480504799f;
567 break;
568
569 case HollowShape.Triangle:
570 hollowVolume *= .5f * 1.27323954473516f;
571 break;
572
573 default:
574 hollowVolume = 0;
575 break;
576 }
577 volume *= (1.0f - hollowVolume);
578 }
579 }
580 break;
581 466
582 case ProfileShape.HalfCircle: 467 public override Vector3 CenterOfMass
583 if (_pbs.PathCurve == (byte)Extrusion.Curve1) 468 {
469 get
470 {
471 lock (_parent_scene.OdeLock)
472 {
473 d.Vector3 dtmp;
474 if (!childPrim && Body != IntPtr.Zero)
584 { 475 {
585 volume *= 0.52359877559829887307710723054658f; 476 dtmp = d.BodyGetPosition(Body);
477 return new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
586 } 478 }
587 break; 479 else if (prim_geom != IntPtr.Zero)
480 {
481 d.Quaternion dq;
482 d.GeomCopyQuaternion(prim_geom, out dq);
483 Quaternion q;
484 q.X = dq.X;
485 q.Y = dq.Y;
486 q.Z = dq.Z;
487 q.W = dq.W;
488
489 Vector3 Ptot = m_OBBOffset * q;
490 dtmp = d.GeomGetPosition(prim_geom);
491 Ptot.X += dtmp.X;
492 Ptot.Y += dtmp.Y;
493 Ptot.Z += dtmp.Z;
494
495 // if(childPrim) we only know about physical linksets
496 return Ptot;
497/*
498 float tmass = _mass;
499 Ptot *= tmass;
588 500
589 case ProfileShape.EquilateralTriangle: 501 float m;
590 502
591 if (_pbs.PathCurve == (byte)Extrusion.Straight) 503 foreach (OdePrim prm in childrenPrim)
592 { 504 {
593 volume *= 0.32475953f; 505 m = prm._mass;
594 506 Ptot += prm.CenterOfMass * m;
595 if (hollowAmount > 0.0) 507 tmass += m;
596 {
597
598 // calculate the hollow volume by it's shape compared to the prim shape
599 switch (_pbs.HollowShape)
600 {
601 case HollowShape.Same:
602 case HollowShape.Triangle:
603 hollowVolume *= .25f;
604 break;
605
606 case HollowShape.Square:
607 hollowVolume *= 0.499849f * 3.07920140172638f;
608 break;
609
610 case HollowShape.Circle:
611 // Hollow shape is a perfect cyllinder in respect to the cube's scale
612 // Cyllinder hollow volume calculation
613
614 hollowVolume *= 0.1963495f * 3.07920140172638f;
615 break;
616
617 default:
618 hollowVolume = 0;
619 break;
620 }
621 volume *= (1.0f - hollowVolume);
622 }
623 } 508 }
624 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
625 {
626 volume *= 0.32475953f;
627 volume *= 0.01f * (float)(200 - _pbs.PathScaleX);
628 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
629 volume *= (1.0f - tmp * tmp);
630
631 if (hollowAmount > 0.0)
632 {
633
634 hollowVolume *= hollowAmount;
635 509
636 switch (_pbs.HollowShape) 510 if (tmass == 0)
637 { 511 tmass = 0;
638 case HollowShape.Same: 512 else
639 case HollowShape.Triangle: 513 tmass = 1.0f / tmass;
640 hollowVolume *= .25f;
641 break;
642
643 case HollowShape.Square:
644 hollowVolume *= 0.499849f * 3.07920140172638f;
645 break;
646
647 case HollowShape.Circle:
648 514
649 hollowVolume *= 0.1963495f * 3.07920140172638f; 515 Ptot *= tmass;
650 break; 516 return Ptot;
517*/
518 }
519 else
520 return _position;
521 }
522 }
523 }
651 524
652 default: 525 public override Vector3 OOBsize
653 hollowVolume = 0; 526 {
654 break; 527 get
655 } 528 {
656 volume *= (1.0f - hollowVolume); 529 return m_OBB;
657 } 530 }
658 } 531 }
659 break;
660 532
661 default: 533 public override Vector3 OOBoffset
662 break; 534 {
535 get
536 {
537 return m_OBBOffset;
663 } 538 }
539 }
664 540
665 float taperX1; 541 public override float OOBRadiusSQ
666 float taperY1; 542 {
667 float taperX; 543 get
668 float taperY; 544 {
669 float pathBegin; 545 return primOOBradiusSQ;
670 float pathEnd; 546 }
671 float profileBegin; 547 }
672 float profileEnd;
673 548
674 if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) 549 public override PrimitiveBaseShape Shape
550 {
551 set
675 { 552 {
676 taperX1 = _pbs.PathScaleX * 0.01f; 553// AddChange(changes.Shape, value);
677 if (taperX1 > 1.0f) 554 _parent_scene.m_meshWorker.ChangeActorPhysRep(this, value, _size, m_shapetype);
678 taperX1 = 2.0f - taperX1; 555 }
679 taperX = 1.0f - taperX1; 556 }
680 557
681 taperY1 = _pbs.PathScaleY * 0.01f; 558 public override byte PhysicsShapeType
682 if (taperY1 > 1.0f) 559 {
683 taperY1 = 2.0f - taperY1; 560 get
684 taperY = 1.0f - taperY1; 561 {
562 return m_shapetype;
685 } 563 }
686 else 564 set
687 { 565 {
688 taperX = _pbs.PathTaperX * 0.01f; 566 m_shapetype = value;
689 if (taperX < 0.0f) 567 _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, _size, value);
690 taperX = -taperX;
691 taperX1 = 1.0f - taperX;
692
693 taperY = _pbs.PathTaperY * 0.01f;
694 if (taperY < 0.0f)
695 taperY = -taperY;
696 taperY1 = 1.0f - taperY;
697 } 568 }
569 }
698 570
699 volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); 571 public override Vector3 Velocity
700 572 {
701 pathBegin = (float)_pbs.PathBegin * 2.0e-5f; 573 get
702 pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; 574 {
703 volume *= (pathEnd - pathBegin); 575 if (_zeroFlag)
704 576 return Vector3.Zero;
705// this is crude aproximation 577 return _velocity;
706 profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; 578 }
707 profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; 579 set
708 volume *= (profileEnd - profileBegin);
709
710 returnMass = m_density * volume;
711
712 if (returnMass <= 0)
713 returnMass = 0.0001f;//ckrinke: Mass must be greater then zero.
714// else if (returnMass > _parent_scene.maximumMassObject)
715// returnMass = _parent_scene.maximumMassObject;
716
717 // Recursively calculate mass
718 bool HasChildPrim = false;
719 lock (childrenPrim)
720 { 580 {
721 if (childrenPrim.Count > 0) 581 if (value.IsFinite())
582 {
583 AddChange(changes.Velocity, value);
584 }
585 else
722 { 586 {
723 HasChildPrim = true; 587 m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name);
724 } 588 }
589
725 } 590 }
591 }
726 592
727 if (HasChildPrim) 593 public override Vector3 Torque
594 {
595 get
728 { 596 {
729 OdePrim[] childPrimArr = new OdePrim[0]; 597 if (!IsPhysical || Body == IntPtr.Zero)
598 return Vector3.Zero;
730 599
731 lock (childrenPrim) 600 return _torque;
732 childPrimArr = childrenPrim.ToArray(); 601 }
733 602
734 for (int i = 0; i < childPrimArr.Length; i++) 603 set
604 {
605 if (value.IsFinite())
735 { 606 {
736 if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove) 607 AddChange(changes.Torque, value);
737 returnMass += childPrimArr[i].CalculateMass(); 608 }
738 // failsafe, this shouldn't happen but with OpenSim, you never know :) 609 else
739 if (i > 256) 610 {
740 break; 611 m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name);
741 } 612 }
742 } 613 }
614 }
743 615
744 if (returnMass > _parent_scene.maximumMassObject) 616 public override float CollisionScore
745 returnMass = _parent_scene.maximumMassObject; 617 {
746 618 get { return m_collisionscore; }
747 return returnMass; 619 set { m_collisionscore = value; }
748 } 620 }
749 621
750 #endregion 622 public override bool Kinematic
623 {
624 get { return false; }
625 set { }
626 }
751 627
752 private void setMass() 628 public override Quaternion Orientation
753 { 629 {
754 if (Body != (IntPtr) 0) 630 get
755 { 631 {
756 float newmass = CalculateMass(); 632 if (givefakeori > 0)
757 633 return fakeori;
758 //m_log.Info("[PHYSICS]: New Mass: " + newmass.ToString()); 634 else
759 635
760 d.MassSetBoxTotal(out pMass, newmass, _size.X, _size.Y, _size.Z); 636 return _orientation;
761 d.BodySetMass(Body, ref pMass);
762 } 637 }
763 } 638 set
764
765 /// <summary>
766 /// Stop a prim from being subject to physics.
767 /// </summary>
768 internal void disableBody()
769 {
770 //this kills the body so things like 'mesh' can re-create it.
771 lock (this)
772 { 639 {
773 if (!childPrim) 640 if (QuaternionIsFinite(value))
774 { 641 {
775 if (Body != IntPtr.Zero) 642 fakeori = value;
776 { 643 givefakeori++;
777 _parent_scene.DeactivatePrim(this);
778 m_collisionCategories &= ~CollisionCategories.Body;
779 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
780 644
781 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 645 value.Normalize();
782 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
783 646
784 d.BodyDestroy(Body); 647 AddChange(changes.Orientation, value);
785 lock (childrenPrim)
786 {
787 if (childrenPrim.Count > 0)
788 {
789 foreach (OdePrim prm in childrenPrim)
790 {
791 _parent_scene.DeactivatePrim(prm);
792 prm.Body = IntPtr.Zero;
793 }
794 }
795 }
796 Body = IntPtr.Zero;
797 }
798 } 648 }
799 else 649 else
800 { 650 m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name);
801 _parent_scene.DeactivatePrim(this);
802
803 m_collisionCategories &= ~CollisionCategories.Body;
804 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
805
806 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
807 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
808 651
809 Body = IntPtr.Zero;
810 }
811 } 652 }
812
813 m_disabled = true;
814 m_collisionscore = 0;
815 } 653 }
816 654
817 private static Dictionary<IMesh, IntPtr> m_MeshToTriMeshMap = new Dictionary<IMesh, IntPtr>(); 655 public override Vector3 Acceleration
818
819 private void setMesh(OdeScene parent_scene, IMesh mesh)
820 { 656 {
821// m_log.DebugFormat("[ODE PRIM]: Setting mesh on {0} to {1}", Name, mesh); 657 get { return _acceleration; }
658 set { }
659 }
822 660
823 // This sleeper is there to moderate how long it takes between 661 public override Vector3 RotationalVelocity
824 // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object 662 {
663 get
664 {
665 Vector3 pv = Vector3.Zero;
666 if (_zeroFlag)
667 return pv;
825 668
826 //Thread.Sleep(10); 669 if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f))
670 return pv;
827 671
828 //Kill Body so that mesh can re-make the geom 672 return m_rotationalVelocity;
829 if (IsPhysical && Body != IntPtr.Zero) 673 }
674 set
830 { 675 {
831 if (childPrim) 676 if (value.IsFinite())
832 { 677 {
833 if (_parent != null) 678 AddChange(changes.AngVelocity, value);
834 {
835 OdePrim parent = (OdePrim)_parent;
836 parent.ChildDelink(this);
837 }
838 } 679 }
839 else 680 else
840 { 681 {
841 disableBody(); 682 m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name);
842 } 683 }
843 } 684 }
685 }
844 686
845 IntPtr vertices, indices; 687 public override float Buoyancy
846 int vertexCount, indexCount; 688 {
847 int vertexStride, triStride; 689 get { return m_buoyancy; }
848 mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap 690 set
849 mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage 691 {
850 m_expectedCollisionContacts = indexCount; 692 AddChange(changes.Buoyancy,value);
851 mesh.releaseSourceMeshData(); // free up the original mesh data to save memory 693 }
694 }
695
696 public override bool FloatOnWater
697 {
698 set
699 {
700 AddChange(changes.CollidesWater, value);
701 }
702 }
852 703
853 // We must lock here since m_MeshToTriMeshMap is static and multiple scene threads may call this method at 704 public override Vector3 PIDTarget
854 // the same time. 705 {
855 lock (m_MeshToTriMeshMap) 706 set
856 { 707 {
857 if (m_MeshToTriMeshMap.ContainsKey(mesh)) 708 if (value.IsFinite())
858 { 709 {
859 _triMeshData = m_MeshToTriMeshMap[mesh]; 710 AddChange(changes.PIDTarget,value);
860 } 711 }
861 else 712 else
713 m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name);
714 }
715 }
716
717 public override bool PIDActive
718 {
719 set
720 {
721 AddChange(changes.PIDActive,value);
722 }
723 }
724
725 public override float PIDTau
726 {
727 set
728 {
729 float tmp = 0;
730 if (value > 0)
862 { 731 {
863 _triMeshData = d.GeomTriMeshDataCreate(); 732 float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep);
864 733 if (value < mint)
865 d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); 734 tmp = mint;
866 d.GeomTriMeshDataPreprocess(_triMeshData); 735 else
867 m_MeshToTriMeshMap[mesh] = _triMeshData; 736 tmp = value;
868 } 737 }
738 AddChange(changes.PIDTau,tmp);
869 } 739 }
740 }
870 741
871// _parent_scene.waitForSpaceUnlock(m_targetSpace); 742 public override float PIDHoverHeight
872 try 743 {
744 set
873 { 745 {
874 SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, parent_scene.triCallback, null, null)); 746 AddChange(changes.PIDHoverHeight,value);
875 } 747 }
876 catch (AccessViolationException) 748 }
749 public override bool PIDHoverActive
750 {
751 set
877 { 752 {
878 m_log.ErrorFormat("[PHYSICS]: MESH LOCKED FOR {0}", Name); 753 AddChange(changes.PIDHoverActive, value);
879 return;
880 } 754 }
755 }
881 756
882 // if (IsPhysical && Body == (IntPtr) 0) 757 public override PIDHoverType PIDHoverType
883 // { 758 {
884 // Recreate the body 759 set
885 // m_interpenetrationcount = 0; 760 {
886 // m_collisionscore = 0; 761 AddChange(changes.PIDHoverType,value);
887 762 }
888 // enableBody();
889 // }
890 } 763 }
891 764
892 internal void ProcessTaints() 765 public override float PIDHoverTau
893 { 766 {
894#if SPAM 767 set
895Console.WriteLine("ZProcessTaints for " + Name); 768 {
896#endif 769 float tmp =0;
770 if (value > 0)
771 {
772 float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep);
773 if (value < mint)
774 tmp = mint;
775 else
776 tmp = value;
777 }
778 AddChange(changes.PIDHoverTau, tmp);
779 }
780 }
897 781
898 // This must be processed as the very first taint so that later operations have a prim_geom to work with 782 public override Quaternion APIDTarget { set { return; } }
899 // if this is a new prim.
900 if (m_taintadd)
901 changeadd();
902 783
903 if (!_position.ApproxEquals(m_taintposition, 0f)) 784 public override bool APIDActive { set { return; } }
904 changemove();
905 785
906 if (m_taintrot != _orientation) 786 public override float APIDStrength { set { return; } }
787
788 public override float APIDDamping { set { return; } }
789
790 public override int VehicleType
791 {
792 // we may need to put a fake on this
793 get
907 { 794 {
908 if (childPrim && IsPhysical) // For physical child prim... 795 if (m_vehicle == null)
909 { 796 return (int)Vehicle.TYPE_NONE;
910 rotate();
911 // KF: ODE will also rotate the parent prim!
912 // so rotate the root back to where it was
913 OdePrim parent = (OdePrim)_parent;
914 parent.rotate();
915 }
916 else 797 else
917 { 798 return (int)m_vehicle.Type;
918 //Just rotate the prim
919 rotate();
920 }
921 } 799 }
922 800 set
923 if (m_taintPhysics != IsPhysical && !(m_taintparent != _parent)) 801 {
924 changePhysicsStatus(); 802 AddChange(changes.VehicleType, value);
803 }
804 }
805
806 public override void VehicleFloatParam(int param, float value)
807 {
808 strVehicleFloatParam fp = new strVehicleFloatParam();
809 fp.param = param;
810 fp.value = value;
811 AddChange(changes.VehicleFloatParam, fp);
812 }
813
814 public override void VehicleVectorParam(int param, Vector3 value)
815 {
816 strVehicleVectorParam fp = new strVehicleVectorParam();
817 fp.param = param;
818 fp.value = value;
819 AddChange(changes.VehicleVectorParam, fp);
820 }
821
822 public override void VehicleRotationParam(int param, Quaternion value)
823 {
824 strVehicleQuatParam fp = new strVehicleQuatParam();
825 fp.param = param;
826 fp.value = value;
827 AddChange(changes.VehicleRotationParam, fp);
828 }
829
830 public override void VehicleFlags(int param, bool value)
831 {
832 strVehicleBoolParam bp = new strVehicleBoolParam();
833 bp.param = param;
834 bp.value = value;
835 AddChange(changes.VehicleFlags, bp);
836 }
837
838 public override void SetVehicle(object vdata)
839 {
840 AddChange(changes.SetVehicle, vdata);
841 }
842 public void SetAcceleration(Vector3 accel)
843 {
844 _acceleration = accel;
845 }
846
847 public override void AddForce(Vector3 force, bool pushforce)
848 {
849 if (force.IsFinite())
850 {
851 if(pushforce)
852 AddChange(changes.AddForce, force);
853 else // a impulse
854 AddChange(changes.AddForce, force * m_invTimeStep);
855 }
856 else
857 {
858 m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name);
859 }
860 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
861 }
925 862
926 if (!_size.ApproxEquals(m_taintsize, 0f)) 863 public override void AddAngularForce(Vector3 force, bool pushforce)
927 changesize(); 864 {
865 if (force.IsFinite())
866 {
867// if(pushforce) for now applyrotationimpulse seems more happy applied as a force
868 AddChange(changes.AddAngForce, force);
869// else // a impulse
870// AddChange(changes.AddAngForce, force * m_invTimeStep);
871 }
872 else
873 {
874 m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name);
875 }
876 }
928 877
929 if (m_taintshape) 878 public override void CrossingFailure()
930 changeshape(); 879 {
880 if (m_outbounds)
881 {
882 _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f);
883 _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f);
884 _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f);
931 885
932 if (m_taintforce) 886 m_lastposition = _position;
933 changeAddForce(); 887 _velocity.X = 0;
888 _velocity.Y = 0;
889 _velocity.Z = 0;
934 890
935 if (m_taintaddangularforce) 891 m_lastVelocity = _velocity;
936 changeAddAngularForce(); 892 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
893 m_vehicle.Stop();
937 894
938 if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f)) 895 if(Body != IntPtr.Zero)
939 changeSetTorque(); 896 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
897 if (prim_geom != IntPtr.Zero)
898 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
940 899
941 if (m_taintdisable) 900 m_outbounds = false;
942 changedisable(); 901 changeDisable(false);
902 base.RequestPhysicsterseUpdate();
903 }
904 }
943 905
944 if (m_taintselected != m_isSelected) 906 public override void SetMomentum(Vector3 momentum)
945 changeSelectedStatus(); 907 {
908 }
946 909
947 if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f)) 910 public override void SetMaterial(int pMaterial)
948 changevelocity(); 911 {
912 m_material = pMaterial;
913 mu = _parent_scene.m_materialContactsData[pMaterial].mu;
914 bounce = _parent_scene.m_materialContactsData[pMaterial].bounce;
915 }
949 916
950 if (m_taintparent != _parent) 917 public void setPrimForRemoval()
951 changelink(); 918 {
919 AddChange(changes.Remove, null);
920 }
952 921
953 if (m_taintCollidesWater != m_collidesWater) 922 public override void link(PhysicsActor obj)
954 changefloatonwater(); 923 {
924 AddChange(changes.Link, obj);
925 }
955 926
956 if (!m_angularlock.ApproxEquals(m_taintAngularLock,0f)) 927 public override void delink()
957 changeAngularLock(); 928 {
929 AddChange(changes.DeLink, null);
958 } 930 }
959 931
960 /// <summary> 932 public override void LockAngularMotion(Vector3 axis)
961 /// Change prim in response to an angular lock taint.
962 /// </summary>
963 private void changeAngularLock()
964 { 933 {
965 // do we have a Physical object? 934 // reverse the zero/non zero values for ODE.
966 if (Body != IntPtr.Zero) 935 if (axis.IsFinite())
967 { 936 {
968 //Check that we have a Parent 937 axis.X = (axis.X > 0) ? 1f : 0f;
969 //If we have a parent then we're not authorative here 938 axis.Y = (axis.Y > 0) ? 1f : 0f;
970 if (_parent == null) 939 axis.Z = (axis.Z > 0) ? 1f : 0f;
971 { 940// m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
972 if (!m_taintAngularLock.ApproxEquals(Vector3.One, 0f)) 941 AddChange(changes.AngLock, axis);
973 {
974 //d.BodySetFiniteRotationMode(Body, 0);
975 //d.BodySetFiniteRotationAxis(Body,m_taintAngularLock.X,m_taintAngularLock.Y,m_taintAngularLock.Z);
976 createAMotor(m_taintAngularLock);
977 }
978 else
979 {
980 if (Amotor != IntPtr.Zero)
981 {
982 d.JointDestroy(Amotor);
983 Amotor = IntPtr.Zero;
984 }
985 }
986 }
987 } 942 }
943 else
944 {
945 m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name);
946 }
947 }
988 948
989 // Store this for later in case we get turned into a separate body 949 public override void SubscribeEvents(int ms)
990 m_angularlock = m_taintAngularLock; 950 {
951 m_eventsubscription = ms;
952 m_cureventsubscription = 0;
953 if (CollisionEventsThisFrame == null)
954 CollisionEventsThisFrame = new CollisionEventUpdate();
955 SentEmptyCollisionsEvent = false;
991 } 956 }
992 957
993 /// <summary> 958 public override void UnSubscribeEvents()
994 /// Change prim in response to a link taint.
995 /// </summary>
996 private void changelink()
997 { 959 {
998 // If the newly set parent is not null 960 if (CollisionEventsThisFrame != null)
999 // create link
1000 if (_parent == null && m_taintparent != null)
1001 { 961 {
1002 if (m_taintparent.PhysicsActorType == (int)ActorTypes.Prim) 962 CollisionEventsThisFrame.Clear();
1003 { 963 CollisionEventsThisFrame = null;
1004 OdePrim obj = (OdePrim)m_taintparent;
1005 //obj.disableBody();
1006//Console.WriteLine("changelink calls ParentPrim");
1007 obj.AddChildPrim(this);
1008
1009 /*
1010 if (obj.Body != (IntPtr)0 && Body != (IntPtr)0 && obj.Body != Body)
1011 {
1012 _linkJointGroup = d.JointGroupCreate(0);
1013 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
1014 d.JointAttach(m_linkJoint, obj.Body, Body);
1015 d.JointSetFixed(m_linkJoint);
1016 }
1017 */
1018 }
1019 } 964 }
1020 // If the newly set parent is null 965 m_eventsubscription = 0;
1021 // destroy link 966 _parent_scene.RemoveCollisionEventReporting(this);
1022 else if (_parent != null && m_taintparent == null) 967 }
968
969 public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
970 {
971 if (CollisionEventsThisFrame == null)
972 CollisionEventsThisFrame = new CollisionEventUpdate();
973// if(CollisionEventsThisFrame.Count < 32)
974 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
975 }
976
977 public void SendCollisions()
978 {
979 if (CollisionEventsThisFrame == null)
980 return;
981
982 if (m_cureventsubscription < m_eventsubscription)
983 return;
984
985 m_cureventsubscription = 0;
986
987 int ncolisions = CollisionEventsThisFrame.m_objCollisionList.Count;
988
989 if (!SentEmptyCollisionsEvent || ncolisions > 0)
1023 { 990 {
1024//Console.WriteLine(" changelink B"); 991 base.SendCollisionUpdate(CollisionEventsThisFrame);
1025 992
1026 if (_parent is OdePrim) 993 if (ncolisions == 0)
1027 { 994 {
1028 OdePrim obj = (OdePrim)_parent; 995 SentEmptyCollisionsEvent = true;
1029 obj.ChildDelink(this); 996 _parent_scene.RemoveCollisionEventReporting(this);
1030 childPrim = false;
1031 //_parent = null;
1032 } 997 }
1033 998 else
1034 /* 999 {
1035 if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0) 1000 SentEmptyCollisionsEvent = false;
1036 d.JointGroupDestroy(_linkJointGroup); 1001 CollisionEventsThisFrame.Clear();
1037 1002 }
1038 _linkJointGroup = (IntPtr)0; 1003 }
1039 m_linkJoint = (IntPtr)0;
1040 */
1041 }
1042
1043 _parent = m_taintparent;
1044 m_taintPhysics = IsPhysical;
1045 } 1004 }
1046 1005
1047 /// <summary> 1006 internal void AddCollisionFrameTime(int t)
1048 /// Add a child prim to this parent prim.
1049 /// </summary>
1050 /// <param name="prim">Child prim</param>
1051 private void AddChildPrim(OdePrim prim)
1052 { 1007 {
1053 if (LocalID == prim.LocalID) 1008 if (m_cureventsubscription < 50000)
1054 return; 1009 m_cureventsubscription += t;
1010 }
1055 1011
1056 if (Body == IntPtr.Zero) 1012 public override bool SubscribedEvents()
1013 {
1014 if (m_eventsubscription > 0)
1015 return true;
1016 return false;
1017 }
1018
1019 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
1020 Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,byte _shapeType,uint plocalID)
1021 {
1022 Name = primName;
1023 LocalID = plocalID;
1024
1025 m_vehicle = null;
1026
1027 if (!pos.IsFinite())
1057 { 1028 {
1058 Body = d.BodyCreate(_parent_scene.world); 1029 pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
1059 setMass(); 1030 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
1031 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name);
1060 } 1032 }
1033 _position = pos;
1034 givefakepos = 0;
1061 1035
1062 lock (childrenPrim) 1036 m_timeStep = parent_scene.ODE_STEPSIZE;
1037 m_invTimeStep = 1f / m_timeStep;
1038
1039 m_density = parent_scene.geomDefaultDensity;
1040 body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
1041
1042 prim_geom = IntPtr.Zero;
1043 collide_geom = IntPtr.Zero;
1044 Body = IntPtr.Zero;
1045
1046 if (!size.IsFinite())
1063 { 1047 {
1064 if (childrenPrim.Contains(prim)) 1048 size = new Vector3(0.5f, 0.5f, 0.5f);
1065 return; 1049 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name);
1050 }
1066 1051
1067// m_log.DebugFormat( 1052 if (size.X <= 0) size.X = 0.01f;
1068// "[ODE PRIM]: Linking prim {0} {1} to {2} {3}", prim.Name, prim.LocalID, Name, LocalID); 1053 if (size.Y <= 0) size.Y = 0.01f;
1054 if (size.Z <= 0) size.Z = 0.01f;
1069 1055
1070 childrenPrim.Add(prim); 1056 _size = size;
1071 1057
1072 foreach (OdePrim prm in childrenPrim) 1058 if (!QuaternionIsFinite(rotation))
1073 { 1059 {
1074 d.Mass m2; 1060 rotation = Quaternion.Identity;
1075 d.MassSetZero(out m2); 1061 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name);
1076 d.MassSetBoxTotal(out m2, prim.CalculateMass(), prm._size.X, prm._size.Y, prm._size.Z); 1062 }
1077 1063
1078 d.Quaternion quat = new d.Quaternion(); 1064 _orientation = rotation;
1079 quat.W = prm._orientation.W; 1065 givefakeori = 0;
1080 quat.X = prm._orientation.X;
1081 quat.Y = prm._orientation.Y;
1082 quat.Z = prm._orientation.Z;
1083 1066
1084 d.Matrix3 mat = new d.Matrix3(); 1067 _pbs = pbs;
1085 d.RfromQ(out mat, ref quat);
1086 d.MassRotate(ref m2, ref mat);
1087 d.MassTranslate(ref m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z);
1088 d.MassAdd(ref pMass, ref m2);
1089 }
1090 1068
1091 foreach (OdePrim prm in childrenPrim) 1069 _parent_scene = parent_scene;
1092 { 1070 m_targetSpace = IntPtr.Zero;
1093 prm.m_collisionCategories |= CollisionCategories.Body;
1094 prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1095 1071
1096//Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + Name); 1072 if (pos.Z < 0)
1097 d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); 1073 {
1098 d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); 1074 m_isphysical = false;
1075 }
1076 else
1077 {
1078 m_isphysical = pisPhysical;
1079 }
1080 m_fakeisphysical = m_isphysical;
1099 1081
1100 d.Quaternion quat = new d.Quaternion(); 1082 m_isVolumeDetect = false;
1101 quat.W = prm._orientation.W; 1083 m_fakeisVolumeDetect = false;
1102 quat.X = prm._orientation.X;
1103 quat.Y = prm._orientation.Y;
1104 quat.Z = prm._orientation.Z;
1105 1084
1106 d.Matrix3 mat = new d.Matrix3(); 1085 m_force = Vector3.Zero;
1107 d.RfromQ(out mat, ref quat);
1108 if (Body != IntPtr.Zero)
1109 {
1110 d.GeomSetBody(prm.prim_geom, Body);
1111 prm.childPrim = true;
1112 d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z);
1113 //d.GeomSetOffsetPosition(prim.prim_geom,
1114 // (Position.X - prm.Position.X) - pMass.c.X,
1115 // (Position.Y - prm.Position.Y) - pMass.c.Y,
1116 // (Position.Z - prm.Position.Z) - pMass.c.Z);
1117 d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat);
1118 //d.GeomSetOffsetRotation(prm.prim_geom, ref mat);
1119 d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
1120 d.BodySetMass(Body, ref pMass);
1121 }
1122 else
1123 {
1124 m_log.DebugFormat("[PHYSICS]: {0} ain't got no boooooooooddy, no body", Name);
1125 }
1126 1086
1127 prm.m_interpenetrationcount = 0; 1087 m_iscolliding = false;
1128 prm.m_collisionscore = 0; 1088 m_colliderfilter = 0;
1129 prm.m_disabled = false; 1089 m_NoColide = false;
1130 1090
1131 // The body doesn't already have a finite rotation mode set here 1091 _triMeshData = IntPtr.Zero;
1132 if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
1133 {
1134 prm.createAMotor(m_angularlock);
1135 }
1136 prm.Body = Body;
1137 _parent_scene.ActivatePrim(prm);
1138 }
1139
1140 m_collisionCategories |= CollisionCategories.Body;
1141 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1142
1143//Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + Name);
1144 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1145//Console.WriteLine(" Post GeomSetCategoryBits 2");
1146 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1147
1148 d.Quaternion quat2 = new d.Quaternion();
1149 quat2.W = _orientation.W;
1150 quat2.X = _orientation.X;
1151 quat2.Y = _orientation.Y;
1152 quat2.Z = _orientation.Z;
1153
1154 d.Matrix3 mat2 = new d.Matrix3();
1155 d.RfromQ(out mat2, ref quat2);
1156 d.GeomSetBody(prim_geom, Body);
1157 d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z);
1158 //d.GeomSetOffsetPosition(prim.prim_geom,
1159 // (Position.X - prm.Position.X) - pMass.c.X,
1160 // (Position.Y - prm.Position.Y) - pMass.c.Y,
1161 // (Position.Z - prm.Position.Z) - pMass.c.Z);
1162 //d.GeomSetOffsetRotation(prim_geom, ref mat2);
1163 d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
1164 d.BodySetMass(Body, ref pMass);
1165
1166 d.BodySetAutoDisableFlag(Body, true);
1167 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
1168
1169 m_interpenetrationcount = 0;
1170 m_collisionscore = 0;
1171 m_disabled = false;
1172 1092
1173 // The body doesn't already have a finite rotation mode set here 1093 m_shapetype = _shapeType;
1174 if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
1175 {
1176 createAMotor(m_angularlock);
1177 }
1178 1094
1179 d.BodySetPosition(Body, Position.X, Position.Y, Position.Z); 1095 m_lastdoneSelected = false;
1096 m_isSelected = false;
1097 m_delaySelect = false;
1180 1098
1181 if (m_vehicle.Type != Vehicle.TYPE_NONE) 1099 m_isphantom = pisPhantom;
1182 m_vehicle.Enable(Body, _parent_scene); 1100 m_fakeisphantom = pisPhantom;
1183 1101
1184 _parent_scene.ActivatePrim(this); 1102 mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu;
1185 } 1103 bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce;
1104
1105 m_building = true; // control must set this to false when done
1106
1107 _parent_scene.m_meshWorker.NewActorPhysRep(this, _pbs, _size, m_shapetype);
1186 } 1108 }
1187 1109
1188 private void ChildSetGeom(OdePrim odePrim) 1110 private void resetCollisionAccounting()
1189 { 1111 {
1190// m_log.DebugFormat( 1112 m_collisionscore = 0;
1191// "[ODE PRIM]: ChildSetGeom {0} {1} for {2} {3}", odePrim.Name, odePrim.LocalID, Name, LocalID); 1113 }
1192 1114
1193 //if (IsPhysical && Body != IntPtr.Zero) 1115 private void UpdateCollisionCatFlags()
1194 lock (childrenPrim) 1116 {
1117 if(m_isphysical && m_disabled)
1195 { 1118 {
1196 foreach (OdePrim prm in childrenPrim) 1119 m_collisionCategories = 0;
1197 { 1120 m_collisionFlags = 0;
1198 //prm.childPrim = true;
1199 prm.disableBody();
1200 //prm.m_taintparent = null;
1201 //prm._parent = null;
1202 //prm.m_taintPhysics = false;
1203 //prm.m_disabled = true;
1204 //prm.childPrim = false;
1205 }
1206 } 1121 }
1207 1122
1208 disableBody(); 1123 else if (m_isSelected)
1124 {
1125 m_collisionCategories = CollisionCategories.Selected;
1126 m_collisionFlags = 0;
1127 }
1209 1128
1210 // Spurious - Body == IntPtr.Zero after disableBody() 1129 else if (m_isVolumeDetect)
1211// if (Body != IntPtr.Zero) 1130 {
1212// { 1131 m_collisionCategories = CollisionCategories.VolumeDtc;
1213// _parent_scene.DeactivatePrim(this); 1132 if (m_isphysical)
1214// } 1133 m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character;
1134 else
1135 m_collisionFlags = 0;
1136 }
1137 else if (m_isphantom)
1138 {
1139 m_collisionCategories = CollisionCategories.Phantom;
1140 if (m_isphysical)
1141 m_collisionFlags = CollisionCategories.Land;
1142 else
1143 m_collisionFlags = 0;
1144 }
1145 else
1146 {
1147 m_collisionCategories = CollisionCategories.Geom;
1148 if (m_isphysical)
1149 m_collisionFlags = m_default_collisionFlagsPhysical;
1150 else
1151 m_collisionFlags = m_default_collisionFlagsNotPhysical;
1152 }
1153 }
1215 1154
1216 lock (childrenPrim) 1155 private void ApplyCollisionCatFlags()
1156 {
1157 if (prim_geom != IntPtr.Zero)
1217 { 1158 {
1218 foreach (OdePrim prm in childrenPrim) 1159 if (!childPrim && childrenPrim.Count > 0)
1160 {
1161 foreach (OdePrim prm in childrenPrim)
1162 {
1163 if (m_isphysical && m_disabled)
1164 {
1165 prm.m_collisionCategories = 0;
1166 prm.m_collisionFlags = 0;
1167 }
1168 else
1169 {
1170 // preserve some
1171 if (prm.m_isSelected)
1172 {
1173 prm.m_collisionCategories = CollisionCategories.Selected;
1174 prm.m_collisionFlags = 0;
1175 }
1176 else if (prm.m_isVolumeDetect)
1177 {
1178 prm.m_collisionCategories = CollisionCategories.VolumeDtc;
1179 if (m_isphysical)
1180 prm.m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character;
1181 else
1182 prm.m_collisionFlags = 0;
1183 }
1184 else if (prm.m_isphantom)
1185 {
1186 prm.m_collisionCategories = CollisionCategories.Phantom;
1187 if (m_isphysical)
1188 prm.m_collisionFlags = CollisionCategories.Land;
1189 else
1190 prm.m_collisionFlags = 0;
1191 }
1192 else
1193 {
1194 prm.m_collisionCategories = m_collisionCategories;
1195 prm.m_collisionFlags = m_collisionFlags;
1196 }
1197 }
1198
1199 if (prm.prim_geom != IntPtr.Zero)
1200 {
1201 if (prm.m_NoColide)
1202 {
1203 d.GeomSetCategoryBits(prm.prim_geom, 0);
1204 if (m_isphysical)
1205 d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land);
1206 else
1207 d.GeomSetCollideBits(prm.prim_geom, 0);
1208 }
1209 else
1210 {
1211 d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories);
1212 d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags);
1213 }
1214 }
1215 }
1216 }
1217
1218 if (m_NoColide)
1219 { 1219 {
1220//Console.WriteLine("ChildSetGeom calls ParentPrim"); 1220 d.GeomSetCategoryBits(prim_geom, 0);
1221 AddChildPrim(prm); 1221 d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land);
1222 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
1223 {
1224 d.GeomSetCategoryBits(collide_geom, 0);
1225 d.GeomSetCollideBits(collide_geom, (uint)CollisionCategories.Land);
1226 }
1227 }
1228 else
1229 {
1230 d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
1231 d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
1232 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
1233 {
1234 d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories);
1235 d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags);
1236 }
1222 } 1237 }
1223 } 1238 }
1224 } 1239 }
1225 1240
1226 private void ChildDelink(OdePrim odePrim) 1241 private void createAMotor(Vector3 axis)
1227 { 1242 {
1228// m_log.DebugFormat( 1243 if (Body == IntPtr.Zero)
1229// "[ODE PRIM]: Delinking prim {0} {1} from {2} {3}", odePrim.Name, odePrim.LocalID, Name, LocalID); 1244 return;
1230 1245
1231 // Okay, we have a delinked child.. need to rebuild the body. 1246 if (Amotor != IntPtr.Zero)
1232 lock (childrenPrim)
1233 { 1247 {
1234 foreach (OdePrim prm in childrenPrim) 1248 d.JointDestroy(Amotor);
1235 { 1249 Amotor = IntPtr.Zero;
1236 prm.childPrim = true;
1237 prm.disableBody();
1238 //prm.m_taintparent = null;
1239 //prm._parent = null;
1240 //prm.m_taintPhysics = false;
1241 //prm.m_disabled = true;
1242 //prm.childPrim = false;
1243 }
1244 } 1250 }
1245 1251
1246 disableBody(); 1252 int axisnum = 3 - (int)(axis.X + axis.Y + axis.Z);
1247 1253
1248 lock (childrenPrim) 1254 if (axisnum <= 0)
1255 return;
1256
1257 // stop it
1258 d.BodySetTorque(Body, 0, 0, 0);
1259 d.BodySetAngularVel(Body, 0, 0, 0);
1260
1261 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
1262 d.JointAttach(Amotor, Body, IntPtr.Zero);
1263
1264 d.JointSetAMotorMode(Amotor, 0);
1265
1266 d.JointSetAMotorNumAxes(Amotor, axisnum);
1267
1268 // get current orientation to lock
1269
1270 d.Quaternion dcur = d.BodyGetQuaternion(Body);
1271 Quaternion curr; // crap convertion between identical things
1272 curr.X = dcur.X;
1273 curr.Y = dcur.Y;
1274 curr.Z = dcur.Z;
1275 curr.W = dcur.W;
1276 Vector3 ax;
1277
1278 int i = 0;
1279 int j = 0;
1280 if (axis.X == 0)
1249 { 1281 {
1250 //Console.WriteLine("childrenPrim.Remove " + odePrim); 1282 ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X
1251 childrenPrim.Remove(odePrim); 1283 // ODE should do this with axis relative to body 1 but seems to fail
1284 d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z);
1285 d.JointSetAMotorAngle(Amotor, 0, 0);
1286 d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, -0.000001f);
1287 d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0.000001f);
1288 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0);
1289 d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f);
1290 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f);
1291 d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f);
1292 d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f);
1293 d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f);
1294 i++;
1295 j = 256; // move to next axis set
1252 } 1296 }
1253 1297
1254 // Spurious - Body == IntPtr.Zero after disableBody() 1298 if (axis.Y == 0)
1255// if (Body != IntPtr.Zero) 1299 {
1256// { 1300 ax = (new Vector3(0, 1, 0)) * curr;
1257// _parent_scene.DeactivatePrim(this); 1301 d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
1258// } 1302 d.JointSetAMotorAngle(Amotor, i, 0);
1303 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f);
1304 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f);
1305 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
1306 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
1307 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
1308 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
1309 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
1310 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
1311 i++;
1312 j += 256;
1313 }
1259 1314
1260 lock (childrenPrim) 1315 if (axis.Z == 0)
1261 { 1316 {
1262 foreach (OdePrim prm in childrenPrim) 1317 ax = (new Vector3(0, 0, 1)) * curr;
1263 { 1318 d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
1264//Console.WriteLine("ChildDelink calls ParentPrim"); 1319 d.JointSetAMotorAngle(Amotor, i, 0);
1265 AddChildPrim(prm); 1320 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f);
1266 } 1321 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f);
1322 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
1323 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
1324 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
1325 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
1326 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
1327 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
1267 } 1328 }
1268 } 1329 }
1269 1330
1270 /// <summary> 1331
1271 /// Change prim in response to a selection taint. 1332 private void SetGeom(IntPtr geom)
1272 /// </summary>
1273 private void changeSelectedStatus()
1274 { 1333 {
1275 if (m_taintselected) 1334 prim_geom = geom;
1335 //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name);
1336 if (prim_geom != IntPtr.Zero)
1276 { 1337 {
1277 m_collisionCategories = CollisionCategories.Selected; 1338
1278 m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); 1339 if (m_NoColide)
1279
1280 // We do the body disable soft twice because 'in theory' a collision could have happened
1281 // in between the disabling and the collision properties setting
1282 // which would wake the physical body up from a soft disabling and potentially cause it to fall
1283 // through the ground.
1284
1285 // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select
1286 // just one part of the assembly, the rest of the assembly is non-selected and still simulating,
1287 // so that causes the selected part to wake up and continue moving.
1288
1289 // even if you select all parts of a jointed assembly, it is not guaranteed that the entire
1290 // assembly will stop simulating during the selection, because of the lack of atomicity
1291 // of select operations (their processing could be interrupted by a thread switch, causing
1292 // simulation to continue before all of the selected object notifications trickle down to
1293 // the physics engine).
1294
1295 // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are
1296 // selected and disabled. then, due to a thread switch, the selection processing is
1297 // interrupted and the physics engine continues to simulate, so the last 50 items, whose
1298 // selection was not yet processed, continues to simulate. this wakes up ALL of the
1299 // first 50 again. then the last 50 are disabled. then the first 50, which were just woken
1300 // up, start simulating again, which in turn wakes up the last 50.
1301
1302 if (IsPhysical)
1303 { 1340 {
1304 disableBodySoft(); 1341 d.GeomSetCategoryBits(prim_geom, 0);
1342 if (m_isphysical)
1343 {
1344 d.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land);
1345 }
1346 else
1347 {
1348 d.GeomSetCollideBits(prim_geom, 0);
1349 d.GeomDisable(prim_geom);
1350 }
1305 } 1351 }
1306 1352 else
1307 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1308 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1309
1310 if (IsPhysical)
1311 { 1353 {
1312 disableBodySoft(); 1354 d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
1355 d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
1313 } 1356 }
1357
1358 UpdatePrimBodyData();
1359 _parent_scene.actor_name_map[prim_geom] = this;
1360
1361/*
1362// debug
1363 d.AABB aabb;
1364 d.GeomGetAABB(prim_geom, out aabb);
1365 float x = aabb.MaxX - aabb.MinX;
1366 float y = aabb.MaxY - aabb.MinY;
1367 float z = aabb.MaxZ - aabb.MinZ;
1368 if( x > 60.0f || y > 60.0f || z > 60.0f)
1369 m_log.WarnFormat("[PHYSICS]: large prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}",
1370 Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString());
1371 else if (x < 0.001f || y < 0.001f || z < 0.001f)
1372 m_log.WarnFormat("[PHYSICS]: small prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}",
1373 Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString());
1374
1375//
1376*/
1377
1314 } 1378 }
1315 else 1379 else
1380 m_log.Warn("Setting bad Geom");
1381 }
1382
1383 private bool GetMeshGeom()
1384 {
1385 IntPtr vertices, indices;
1386 int vertexCount, indexCount;
1387 int vertexStride, triStride;
1388
1389 IMesh mesh = m_mesh;
1390
1391 if (mesh == null)
1392 return false;
1393
1394 mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount);
1395 mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount);
1396
1397 if (vertexCount == 0 || indexCount == 0)
1316 { 1398 {
1317 m_collisionCategories = CollisionCategories.Geom; 1399 m_log.WarnFormat("[PHYSICS]: Invalid mesh data on OdePrim {0}, mesh {1}",
1400 Name, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh");
1401
1402 m_hasOBB = false;
1403 m_OBBOffset = Vector3.Zero;
1404 m_OBB = _size * 0.5f;
1405
1406 m_physCost = 0.1f;
1407 m_streamCost = 1.0f;
1408
1409 _parent_scene.mesher.ReleaseMesh(mesh);
1410 m_meshState = MeshState.MeshFailed;
1411 m_mesh = null;
1412 return false;
1413 }
1318 1414
1319 if (IsPhysical) 1415 IntPtr geo = IntPtr.Zero;
1320 m_collisionCategories |= CollisionCategories.Body;
1321 1416
1322 m_collisionFlags = m_default_collisionFlags; 1417 try
1418 {
1419 _triMeshData = d.GeomTriMeshDataCreate();
1323 1420
1324 if (m_collidesLand) 1421 d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
1325 m_collisionFlags |= CollisionCategories.Land; 1422 d.GeomTriMeshDataPreprocess(_triMeshData);
1326 if (m_collidesWater)
1327 m_collisionFlags |= CollisionCategories.Water;
1328 1423
1329 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 1424 geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null);
1330 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 1425 }
1331 1426
1332 if (IsPhysical) 1427 catch (Exception e)
1428 {
1429 m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e);
1430 if (_triMeshData != IntPtr.Zero)
1333 { 1431 {
1334 if (Body != IntPtr.Zero) 1432 try
1433 {
1434 d.GeomTriMeshDataDestroy(_triMeshData);
1435 }
1436 catch
1335 { 1437 {
1336 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1337 d.BodySetForce(Body, 0, 0, 0);
1338 enableBodySoft();
1339 } 1438 }
1340 } 1439 }
1440 _triMeshData = IntPtr.Zero;
1441
1442 m_hasOBB = false;
1443 m_OBBOffset = Vector3.Zero;
1444 m_OBB = _size * 0.5f;
1445 m_physCost = 0.1f;
1446 m_streamCost = 1.0f;
1447
1448 _parent_scene.mesher.ReleaseMesh(mesh);
1449 m_meshState = MeshState.MeshFailed;
1450 m_mesh = null;
1451 return false;
1341 } 1452 }
1342 1453
1343 resetCollisionAccounting(); 1454 m_physCost = 0.0013f * (float)indexCount;
1344 m_isSelected = m_taintselected; 1455 // todo
1345 }//end changeSelectedStatus 1456 m_streamCost = 1.0f;
1346 1457
1347 internal void ResetTaints() 1458 SetGeom(geo);
1348 { 1459
1349 m_taintposition = _position; 1460 return true;
1350 m_taintrot = _orientation;
1351 m_taintPhysics = IsPhysical;
1352 m_taintselected = m_isSelected;
1353 m_taintsize = _size;
1354 m_taintshape = false;
1355 m_taintforce = false;
1356 m_taintdisable = false;
1357 m_taintVelocity = Vector3.Zero;
1358 } 1461 }
1359 1462
1360 /// <summary> 1463 private void CreateGeom()
1361 /// Create a geometry for the given mesh in the given target space.
1362 /// </summary>
1363 /// <param name="m_targetSpace"></param>
1364 /// <param name="mesh">If null, then a mesh is used that is based on the profile shape data.</param>
1365 private void CreateGeom(IntPtr m_targetSpace, IMesh mesh)
1366 { 1464 {
1367#if SPAM 1465 bool hasMesh = false;
1368Console.WriteLine("CreateGeom:"); 1466
1369#endif 1467 m_NoColide = false;
1370 if (mesh != null) 1468
1469 if ((m_meshState & MeshState.FailMask) != 0)
1470 m_NoColide = true;
1471
1472 else if(m_mesh != null)
1371 { 1473 {
1372 setMesh(_parent_scene, mesh); 1474 if (GetMeshGeom())
1475 hasMesh = true;
1476 else
1477 m_NoColide = true;
1373 } 1478 }
1374 else 1479
1480
1481 if (!hasMesh)
1375 { 1482 {
1376 if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1) 1483 IntPtr geo = IntPtr.Zero;
1377 { 1484
1378 if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z) 1485 if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1
1486 && _size.X == _size.Y && _size.Y == _size.Z)
1487 { // it's a sphere
1488 try
1379 { 1489 {
1380 if (((_size.X / 2f) > 0f)) 1490 geo = d.CreateSphere(m_targetSpace, _size.X * 0.5f);
1381 {
1382// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1383 try
1384 {
1385//Console.WriteLine(" CreateGeom 1");
1386 SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2));
1387 m_expectedCollisionContacts = 3;
1388 }
1389 catch (AccessViolationException)
1390 {
1391 m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
1392 return;
1393 }
1394 }
1395 else
1396 {
1397// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1398 try
1399 {
1400//Console.WriteLine(" CreateGeom 2");
1401 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1402 m_expectedCollisionContacts = 4;
1403 }
1404 catch (AccessViolationException)
1405 {
1406 m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
1407 return;
1408 }
1409 }
1410 } 1491 }
1411 else 1492 catch (Exception e)
1412 { 1493 {
1413// _parent_scene.waitForSpaceUnlock(m_targetSpace); 1494 m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e);
1414 try 1495 return;
1415 {
1416//Console.WriteLine(" CreateGeom 3");
1417 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1418 m_expectedCollisionContacts = 4;
1419 }
1420 catch (AccessViolationException)
1421 {
1422 m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
1423 return;
1424 }
1425 } 1496 }
1426 } 1497 }
1427 else 1498 else
1428 { 1499 {// do it as a box
1429// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1430 try 1500 try
1431 { 1501 {
1432//Console.WriteLine(" CreateGeom 4"); 1502 geo = d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z);
1433 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1434 m_expectedCollisionContacts = 4;
1435 } 1503 }
1436 catch (AccessViolationException) 1504 catch (Exception e)
1437 { 1505 {
1438 m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name); 1506 m_log.Warn("[PHYSICS]: Create box failed: {0}", e);
1439 return; 1507 return;
1440 } 1508 }
1441 } 1509 }
1510 m_physCost = 0.1f;
1511 m_streamCost = 1.0f;
1512 SetGeom(geo);
1442 } 1513 }
1443 } 1514 }
1444 1515
1445 /// <summary> 1516 private void RemoveGeom()
1446 /// Remove the existing geom from this prim.
1447 /// </summary>
1448 /// <param name="m_targetSpace"></param>
1449 /// <param name="mesh">If null, then a mesh is used that is based on the profile shape data.</param>
1450 /// <returns>true if the geom was successfully removed, false if it was already gone or the remove failed.</returns>
1451 internal bool RemoveGeom()
1452 { 1517 {
1453 if (prim_geom != IntPtr.Zero) 1518 if (prim_geom != IntPtr.Zero)
1454 { 1519 {
1520 _parent_scene.actor_name_map.Remove(prim_geom);
1521
1455 try 1522 try
1456 { 1523 {
1457 _parent_scene.geom_name_map.Remove(prim_geom);
1458 _parent_scene.actor_name_map.Remove(prim_geom);
1459 d.GeomDestroy(prim_geom); 1524 d.GeomDestroy(prim_geom);
1460 m_expectedCollisionContacts = 0; 1525 if (_triMeshData != IntPtr.Zero)
1461 prim_geom = IntPtr.Zero; 1526 {
1527 d.GeomTriMeshDataDestroy(_triMeshData);
1528 _triMeshData = IntPtr.Zero;
1529 }
1462 } 1530 }
1463 catch (System.AccessViolationException) 1531 catch (Exception e)
1464 { 1532 {
1465 prim_geom = IntPtr.Zero; 1533 m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e);
1466 m_expectedCollisionContacts = 0;
1467 m_log.ErrorFormat("[PHYSICS]: PrimGeom dead for {0}", Name);
1468
1469 return false;
1470 } 1534 }
1471 1535
1472 return true; 1536 prim_geom = IntPtr.Zero;
1537 collide_geom = IntPtr.Zero;
1538 m_targetSpace = IntPtr.Zero;
1473 } 1539 }
1474 else 1540 else
1475 { 1541 {
1476 m_log.WarnFormat( 1542 m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name);
1477 "[ODE PRIM]: Called RemoveGeom() on {0} {1} where geometry was already null.", Name, LocalID); 1543 }
1478 1544
1479 return false; 1545 lock (m_meshlock)
1546 {
1547 if (m_mesh != null)
1548 {
1549 _parent_scene.mesher.ReleaseMesh(m_mesh);
1550 m_mesh = null;
1551 }
1480 } 1552 }
1553
1554 Body = IntPtr.Zero;
1555 m_hasOBB = false;
1481 } 1556 }
1482 /// <summary> 1557
1483 /// Add prim in response to an add taint. 1558 //sets non physical prim m_targetSpace to right space in spaces grid for static prims
1484 /// </summary> 1559 // should only be called for non physical prims unless they are becoming non physical
1485 private void changeadd() 1560 private void SetInStaticSpace(OdePrim prim)
1486 { 1561 {
1487// m_log.DebugFormat("[ODE PRIM]: Adding prim {0}", Name); 1562 IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace);
1488 1563 prim.m_targetSpace = targetSpace;
1489 int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position); 1564 collide_geom = IntPtr.Zero;
1490 IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position); 1565 }
1491 1566
1492 if (targetspace == IntPtr.Zero) 1567 public void enableBodySoft()
1493 targetspace = _parent_scene.createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]); 1568 {
1569 m_disabled = false;
1570 if (!childPrim && !m_isSelected)
1571 {
1572 if (m_isphysical && Body != IntPtr.Zero)
1573 {
1574 UpdateCollisionCatFlags();
1575 ApplyCollisionCatFlags();
1494 1576
1495 m_targetSpace = targetspace; 1577 d.BodyEnable(Body);
1578 }
1579 }
1580 resetCollisionAccounting();
1581 }
1496 1582
1497 IMesh mesh = null; 1583 private void disableBodySoft()
1584 {
1585 m_disabled = true;
1586 if (!childPrim)
1587 {
1588 if (m_isphysical && Body != IntPtr.Zero)
1589 {
1590 if (m_isSelected)
1591 m_collisionFlags = CollisionCategories.Selected;
1592 else
1593 m_collisionCategories = 0;
1594 m_collisionFlags = 0;
1595 ApplyCollisionCatFlags();
1596 d.BodyDisable(Body);
1597 }
1598 }
1599 }
1600
1601 private void MakeBody()
1602 {
1603 if (!m_isphysical) // only physical get bodies
1604 return;
1605
1606 if (childPrim) // child prims don't get bodies;
1607 return;
1608
1609 if (m_building)
1610 return;
1611
1612 if (prim_geom == IntPtr.Zero)
1613 {
1614 m_log.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet");
1615 return;
1616 }
1498 1617
1499 if (_parent_scene.needsMeshing(_pbs)) 1618 if (Body != IntPtr.Zero)
1500 { 1619 {
1501 // Don't need to re-enable body.. it's done in SetMesh 1620 DestroyBody();
1502 mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical); 1621 m_log.Warn("[PHYSICS]: MakeBody called having a body");
1503 // createmesh returns null when it's a shape that isn't a cube.
1504 // m_log.Debug(m_localID);
1505 if (mesh == null)
1506 CheckMeshAsset();
1507 } 1622 }
1508 1623
1509#if SPAM 1624 if (d.GeomGetBody(prim_geom) != IntPtr.Zero)
1510Console.WriteLine("changeadd 1"); 1625 {
1511#endif 1626 d.GeomSetBody(prim_geom, IntPtr.Zero);
1512 CreateGeom(m_targetSpace, mesh); 1627 m_log.Warn("[PHYSICS]: MakeBody root geom already had a body");
1628 }
1513 1629
1514 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); 1630 d.Matrix3 mymat = new d.Matrix3();
1515 d.Quaternion myrot = new d.Quaternion(); 1631 d.Quaternion myrot = new d.Quaternion();
1632 d.Mass objdmass = new d.Mass { };
1633
1634 Body = d.BodyCreate(_parent_scene.world);
1635
1636 objdmass = primdMass;
1637
1638 // rotate inertia
1516 myrot.X = _orientation.X; 1639 myrot.X = _orientation.X;
1517 myrot.Y = _orientation.Y; 1640 myrot.Y = _orientation.Y;
1518 myrot.Z = _orientation.Z; 1641 myrot.Z = _orientation.Z;
1519 myrot.W = _orientation.W; 1642 myrot.W = _orientation.W;
1520 d.GeomSetQuaternion(prim_geom, ref myrot);
1521 1643
1522 if (IsPhysical && Body == IntPtr.Zero) 1644 d.RfromQ(out mymat, ref myrot);
1523 enableBody(); 1645 d.MassRotate(ref objdmass, ref mymat);
1524 1646
1525 changeSelectedStatus(); 1647 // set the body rotation
1526 1648 d.BodySetRotation(Body, ref mymat);
1527 m_taintadd = false;
1528 }
1529 1649
1530 /// <summary> 1650 // recompute full object inertia if needed
1531 /// Move prim in response to a move taint. 1651 if (childrenPrim.Count > 0)
1532 /// </summary>
1533 private void changemove()
1534 {
1535 if (IsPhysical)
1536 { 1652 {
1537 if (!m_disabled && !m_taintremove && !childPrim) 1653 d.Matrix3 mat = new d.Matrix3();
1538 { 1654 d.Quaternion quat = new d.Quaternion();
1539 if (Body == IntPtr.Zero) 1655 d.Mass tmpdmass = new d.Mass { };
1540 enableBody(); 1656 Vector3 rcm;
1541 1657
1542 //Prim auto disable after 20 frames, 1658 rcm.X = _position.X;
1543 //if you move it, re-enable the prim manually. 1659 rcm.Y = _position.Y;
1544 if (_parent != null) 1660 rcm.Z = _position.Z;
1545 {
1546 if (m_linkJoint != IntPtr.Zero)
1547 {
1548 d.JointDestroy(m_linkJoint);
1549 m_linkJoint = IntPtr.Zero;
1550 }
1551 }
1552 1661
1553 if (Body != IntPtr.Zero) 1662 lock (childrenPrim)
1663 {
1664 foreach (OdePrim prm in childrenPrim)
1554 { 1665 {
1555 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z); 1666 if (prm.prim_geom == IntPtr.Zero)
1556
1557 if (_parent != null)
1558 { 1667 {
1559 OdePrim odParent = (OdePrim)_parent; 1668 m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet");
1560 if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body) 1669 continue;
1561 {
1562// KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used??
1563Console.WriteLine(" JointCreateFixed");
1564 m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
1565 d.JointAttach(m_linkJoint, Body, odParent.Body);
1566 d.JointSetFixed(m_linkJoint);
1567 }
1568 } 1670 }
1569 d.BodyEnable(Body); 1671
1570 if (m_vehicle.Type != Vehicle.TYPE_NONE) 1672 tmpdmass = prm.primdMass;
1673
1674 // apply prim current rotation to inertia
1675 quat.X = prm._orientation.X;
1676 quat.Y = prm._orientation.Y;
1677 quat.Z = prm._orientation.Z;
1678 quat.W = prm._orientation.W;
1679 d.RfromQ(out mat, ref quat);
1680 d.MassRotate(ref tmpdmass, ref mat);
1681
1682 Vector3 ppos = prm._position;
1683 ppos.X -= rcm.X;
1684 ppos.Y -= rcm.Y;
1685 ppos.Z -= rcm.Z;
1686 // refer inertia to root prim center of mass position
1687 d.MassTranslate(ref tmpdmass,
1688 ppos.X,
1689 ppos.Y,
1690 ppos.Z);
1691
1692 d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia
1693 // fix prim colision cats
1694
1695 if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero)
1571 { 1696 {
1572 m_vehicle.Enable(Body, _parent_scene); 1697 d.GeomSetBody(prm.prim_geom, IntPtr.Zero);
1698 m_log.Warn("[PHYSICS]: MakeBody child geom already had a body");
1573 } 1699 }
1574 } 1700
1575 else 1701 d.GeomClearOffset(prm.prim_geom);
1576 { 1702 d.GeomSetBody(prm.prim_geom, Body);
1577 m_log.WarnFormat("[PHYSICS]: Body for {0} still null after enableBody(). This is a crash scenario.", Name); 1703 prm.Body = Body;
1704 d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation
1578 } 1705 }
1579 } 1706 }
1580 //else
1581 // {
1582 //m_log.Debug("[BUG]: race!");
1583 //}
1584 } 1707 }
1585 1708
1586 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); 1709 d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset
1587 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); 1710 // associate root geom with body
1588// _parent_scene.waitForSpaceUnlock(m_targetSpace); 1711 d.GeomSetBody(prim_geom, Body);
1712
1713 d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z);
1714 d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z);
1589 1715
1590 IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, _position, m_targetSpace); 1716 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
1591 m_targetSpace = tempspace; 1717 myrot.X = -myrot.X;
1718 myrot.Y = -myrot.Y;
1719 myrot.Z = -myrot.Z;
1592 1720
1593// _parent_scene.waitForSpaceUnlock(m_targetSpace); 1721 d.RfromQ(out mymat, ref myrot);
1722 d.MassRotate(ref objdmass, ref mymat);
1594 1723
1595 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); 1724 d.BodySetMass(Body, ref objdmass);
1725 _mass = objdmass.mass;
1726
1727 // disconnect from world gravity so we can apply buoyancy
1728 d.BodySetGravityMode(Body, false);
1729
1730 d.BodySetAutoDisableFlag(Body, true);
1731 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
1732 d.BodySetDamping(Body, .005f, .005f);
1733
1734 if (m_targetSpace != IntPtr.Zero)
1735 {
1736 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1737 if (d.SpaceQuery(m_targetSpace, prim_geom))
1738 d.SpaceRemove(m_targetSpace, prim_geom);
1739 }
1740
1741 if (childrenPrim.Count == 0)
1742 {
1743 collide_geom = prim_geom;
1744 m_targetSpace = _parent_scene.ActiveSpace;
1745 }
1746 else
1747 {
1748 m_targetSpace = d.HashSpaceCreate(_parent_scene.ActiveSpace);
1749 d.HashSpaceSetLevels(m_targetSpace, -2, 8);
1750 d.SpaceSetSublevel(m_targetSpace, 3);
1751 d.SpaceSetCleanup(m_targetSpace, false);
1752
1753 d.GeomSetCategoryBits(m_targetSpace, (uint)(CollisionCategories.Space |
1754 CollisionCategories.Geom |
1755 CollisionCategories.Phantom |
1756 CollisionCategories.VolumeDtc
1757 ));
1758 d.GeomSetCollideBits(m_targetSpace, 0);
1759 collide_geom = m_targetSpace;
1760 }
1596 1761
1597// _parent_scene.waitForSpaceUnlock(m_targetSpace);
1598 d.SpaceAdd(m_targetSpace, prim_geom); 1762 d.SpaceAdd(m_targetSpace, prim_geom);
1599 1763
1600 changeSelectedStatus(); 1764 if (m_delaySelect)
1765 {
1766 m_isSelected = true;
1767 m_delaySelect = false;
1768 }
1601 1769
1602 resetCollisionAccounting(); 1770 m_collisionscore = 0;
1603 m_taintposition = _position;
1604 }
1605 1771
1606 internal void Move(float timestep) 1772 UpdateCollisionCatFlags();
1607 { 1773 ApplyCollisionCatFlags();
1608 float fx = 0;
1609 float fy = 0;
1610 float fz = 0;
1611 1774
1612 if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims. 1775 _parent_scene.addActivePrim(this);
1776
1777 lock (childrenPrim)
1613 { 1778 {
1614 if (m_vehicle.Type != Vehicle.TYPE_NONE) 1779 foreach (OdePrim prm in childrenPrim)
1615 {
1616 // 'VEHICLES' are dealt with in ODEDynamics.cs
1617 m_vehicle.Step(timestep, _parent_scene);
1618 }
1619 else
1620 { 1780 {
1621//Console.WriteLine("Move " + Name); 1781 if (prm.prim_geom == IntPtr.Zero)
1622 if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009 1782 continue;
1623 // NON-'VEHICLES' are dealt with here
1624// if (d.BodyIsEnabled(Body) && !m_angularlock.ApproxEquals(Vector3.Zero, 0.003f))
1625// {
1626// d.Vector3 avel2 = d.BodyGetAngularVel(Body);
1627// /*
1628// if (m_angularlock.X == 1)
1629// avel2.X = 0;
1630// if (m_angularlock.Y == 1)
1631// avel2.Y = 0;
1632// if (m_angularlock.Z == 1)
1633// avel2.Z = 0;
1634// d.BodySetAngularVel(Body, avel2.X, avel2.Y, avel2.Z);
1635// */
1636// }
1637 //float PID_P = 900.0f;
1638
1639 float m_mass = CalculateMass();
1640
1641// fz = 0f;
1642 //m_log.Info(m_collisionFlags.ToString());
1643 1783
1644 1784 Vector3 ppos = prm._position;
1645 //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. 1785 d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position
1646 // would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ??
1647 // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
1648 // gravityz multiplier = 1 - m_buoyancy
1649 fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass;
1650 1786
1651 if (m_usePID) 1787 if (prm.m_targetSpace != m_targetSpace)
1652 { 1788 {
1653//Console.WriteLine("PID " + Name); 1789 if (prm.m_targetSpace != IntPtr.Zero)
1654 // KF - this is for object move? eg. llSetPos() ?
1655 //if (!d.BodyIsEnabled(Body))
1656 //d.BodySetForce(Body, 0f, 0f, 0f);
1657 // If we're using the PID controller, then we have no gravity
1658 //fz = (-1 * _parent_scene.gravityz) * m_mass; //KF: ?? Prims have no global gravity,so simply...
1659 fz = 0f;
1660
1661 // no lock; for now it's only called from within Simulate()
1662
1663 // If the PID Controller isn't active then we set our force
1664 // calculating base velocity to the current position
1665
1666 if ((m_PIDTau < 1) && (m_PIDTau != 0))
1667 {
1668 //PID_G = PID_G / m_PIDTau;
1669 m_PIDTau = 1;
1670 }
1671
1672 if ((PID_G - m_PIDTau) <= 0)
1673 { 1790 {
1674 PID_G = m_PIDTau + 1; 1791 _parent_scene.waitForSpaceUnlock(prm.m_targetSpace);
1792 if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom))
1793 d.SpaceRemove(prm.m_targetSpace, prm.prim_geom);
1675 } 1794 }
1676 //PidStatus = true; 1795 prm.m_targetSpace = m_targetSpace;
1796 d.SpaceAdd(m_targetSpace, prm.prim_geom);
1797 }
1677 1798
1678 // PhysicsVector vec = new PhysicsVector(); 1799 prm.m_collisionscore = 0;
1679 d.Vector3 vel = d.BodyGetLinearVel(Body);
1680 1800
1681 d.Vector3 pos = d.BodyGetPosition(Body); 1801 if(!m_disabled)
1682 _target_velocity = 1802 prm.m_disabled = false;
1683 new Vector3(
1684 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
1685 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
1686 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
1687 );
1688 1803
1689 // if velocity is zero, use position control; otherwise, velocity control 1804 _parent_scene.addActivePrim(prm);
1805 }
1806 }
1690 1807
1691 if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f)) 1808 // The body doesn't already have a finite rotation mode set here
1692 { 1809 if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null)
1693 // keep track of where we stopped. No more slippin' & slidin' 1810 {
1694 1811 createAMotor(m_angularlock);
1695 // We only want to deactivate the PID Controller if we think we want to have our surrogate 1812 }
1696 // react to the physics scene by moving it's position.
1697 // Avatar to Avatar collisions
1698 // Prim to avatar collisions
1699
1700 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
1701 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
1702 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
1703 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
1704 d.BodySetLinearVel(Body, 0, 0, 0);
1705 d.BodyAddForce(Body, 0, 0, fz);
1706 return;
1707 }
1708 else
1709 {
1710 _zeroFlag = false;
1711 1813
1712 // We're flying and colliding with something
1713 fx = ((_target_velocity.X) - vel.X) * (PID_D);
1714 fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
1715
1716 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
1717 1814
1718 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); 1815 if (m_isSelected || m_disabled)
1719 } 1816 {
1720 } // end if (m_usePID) 1817 d.BodyDisable(Body);
1818 }
1819 else
1820 {
1821 d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z);
1822 d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
1823 }
1824 _parent_scene.addActiveGroups(this);
1825 }
1721 1826
1722 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller 1827 private void DestroyBody()
1723 if (m_useHoverPID && !m_usePID) 1828 {
1724 { 1829 if (Body != IntPtr.Zero)
1725//Console.WriteLine("Hover " + Name); 1830 {
1726 1831 _parent_scene.remActivePrim(this);
1727 // If we're using the PID controller, then we have no gravity
1728 fz = (-1 * _parent_scene.gravityz) * m_mass;
1729 1832
1730 // no lock; for now it's only called from within Simulate() 1833 collide_geom = IntPtr.Zero;
1731 1834
1732 // If the PID Controller isn't active then we set our force 1835 if (m_disabled)
1733 // calculating base velocity to the current position 1836 m_collisionCategories = 0;
1837 else if (m_isSelected)
1838 m_collisionCategories = CollisionCategories.Selected;
1839 else if (m_isVolumeDetect)
1840 m_collisionCategories = CollisionCategories.VolumeDtc;
1841 else if (m_isphantom)
1842 m_collisionCategories = CollisionCategories.Phantom;
1843 else
1844 m_collisionCategories = CollisionCategories.Geom;
1734 1845
1735 if ((m_PIDTau < 1)) 1846 m_collisionFlags = 0;
1736 { 1847
1737 PID_G = PID_G / m_PIDTau; 1848 if (prim_geom != IntPtr.Zero)
1738 } 1849 {
1850 if (m_NoColide)
1851 {
1852 d.GeomSetCategoryBits(prim_geom, 0);
1853 d.GeomSetCollideBits(prim_geom, 0);
1854 }
1855 else
1856 {
1857 d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
1858 d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
1859 }
1860 UpdateDataFromGeom();
1861 d.GeomSetBody(prim_geom, IntPtr.Zero);
1862 SetInStaticSpace(this);
1863 }
1739 1864
1740 if ((PID_G - m_PIDTau) <= 0) 1865 if (!childPrim)
1866 {
1867 lock (childrenPrim)
1868 {
1869 foreach (OdePrim prm in childrenPrim)
1741 { 1870 {
1742 PID_G = m_PIDTau + 1; 1871 _parent_scene.remActivePrim(prm);
1743 }
1744 1872
1745 // Where are we, and where are we headed? 1873 if (prm.m_isSelected)
1746 d.Vector3 pos = d.BodyGetPosition(Body); 1874 prm.m_collisionCategories = CollisionCategories.Selected;
1747 d.Vector3 vel = d.BodyGetLinearVel(Body); 1875 else if (prm.m_isVolumeDetect)
1876 prm.m_collisionCategories = CollisionCategories.VolumeDtc;
1877 else if (prm.m_isphantom)
1878 prm.m_collisionCategories = CollisionCategories.Phantom;
1879 else
1880 prm.m_collisionCategories = CollisionCategories.Geom;
1748 1881
1749 // Non-Vehicles have a limited set of Hover options. 1882 prm.m_collisionFlags = 0;
1750 // determine what our target height really is based on HoverType 1883
1751 switch (m_PIDHoverType) 1884 if (prm.prim_geom != IntPtr.Zero)
1752 { 1885 {
1753 case PIDHoverType.Ground: 1886 if (prm.m_NoColide)
1754 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
1755 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
1756 break;
1757 case PIDHoverType.GroundAndWater:
1758 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
1759 m_waterHeight = _parent_scene.GetWaterLevel();
1760 if (m_groundHeight > m_waterHeight)
1761 { 1887 {
1762 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; 1888 d.GeomSetCategoryBits(prm.prim_geom, 0);
1889 d.GeomSetCollideBits(prm.prim_geom, 0);
1763 } 1890 }
1764 else 1891 else
1765 { 1892 {
1766 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; 1893 d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories);
1894 d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags);
1767 } 1895 }
1768 break; 1896 prm.UpdateDataFromGeom();
1897 SetInStaticSpace(prm);
1898 }
1899 prm.Body = IntPtr.Zero;
1900 prm._mass = prm.primMass;
1901 prm.m_collisionscore = 0;
1902 }
1903 }
1904 if (Amotor != IntPtr.Zero)
1905 {
1906 d.JointDestroy(Amotor);
1907 Amotor = IntPtr.Zero;
1908 }
1909 _parent_scene.remActiveGroup(this);
1910 d.BodyDestroy(Body);
1911 }
1912 Body = IntPtr.Zero;
1913 }
1914 _mass = primMass;
1915 m_collisionscore = 0;
1916 }
1769 1917
1770 } // end switch (m_PIDHoverType) 1918 private void FixInertia(Vector3 NewPos,Quaternion newrot)
1919 {
1920 d.Matrix3 mat = new d.Matrix3();
1921 d.Quaternion quat = new d.Quaternion();
1771 1922
1923 d.Mass tmpdmass = new d.Mass { };
1924 d.Mass objdmass = new d.Mass { };
1772 1925
1773 _target_velocity = 1926 d.BodyGetMass(Body, out tmpdmass);
1774 new Vector3(0.0f, 0.0f, 1927 objdmass = tmpdmass;
1775 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
1776 );
1777 1928
1778 // if velocity is zero, use position control; otherwise, velocity control 1929 d.Vector3 dobjpos;
1930 d.Vector3 thispos;
1779 1931
1780 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) 1932 // get current object position and rotation
1781 { 1933 dobjpos = d.BodyGetPosition(Body);
1782 // keep track of where we stopped. No more slippin' & slidin'
1783
1784 // We only want to deactivate the PID Controller if we think we want to have our surrogate
1785 // react to the physics scene by moving it's position.
1786 // Avatar to Avatar collisions
1787 // Prim to avatar collisions
1788
1789 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
1790 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
1791 d.BodyAddForce(Body, 0, 0, fz);
1792 return;
1793 }
1794 else
1795 {
1796 _zeroFlag = false;
1797 1934
1798 // We're flying and colliding with something 1935 // get prim own inertia in its local frame
1799 fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass); 1936 tmpdmass = primdMass;
1800 }
1801 }
1802 1937
1803 fx *= m_mass; 1938 // transform to object frame
1804 fy *= m_mass; 1939 mat = d.GeomGetOffsetRotation(prim_geom);
1805 //fz *= m_mass; 1940 d.MassRotate(ref tmpdmass, ref mat);
1806 1941
1807 fx += m_force.X; 1942 thispos = d.GeomGetOffsetPosition(prim_geom);
1808 fy += m_force.Y; 1943 d.MassTranslate(ref tmpdmass,
1809 fz += m_force.Z; 1944 thispos.X,
1945 thispos.Y,
1946 thispos.Z);
1810 1947
1811 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); 1948 // subtract current prim inertia from object
1812 if (fx != 0 || fy != 0 || fz != 0) 1949 DMassSubPartFromObj(ref tmpdmass, ref objdmass);
1813 {
1814 //m_taintdisable = true;
1815 //base.RaiseOutOfBounds(Position);
1816 //d.BodySetLinearVel(Body, fx, fy, 0f);
1817 if (!d.BodyIsEnabled(Body))
1818 {
1819 // A physical body at rest on a surface will auto-disable after a while,
1820 // this appears to re-enable it incase the surface it is upon vanishes,
1821 // and the body should fall again.
1822 d.BodySetLinearVel(Body, 0f, 0f, 0f);
1823 d.BodySetForce(Body, 0, 0, 0);
1824 enableBodySoft();
1825 }
1826 1950
1827 // 35x10 = 350n times the mass per second applied maximum. 1951 // back prim own inertia
1828 float nmax = 35f * m_mass; 1952 tmpdmass = primdMass;
1829 float nmin = -35f * m_mass; 1953
1830 1954 // update to new position and orientation
1831 if (fx > nmax) 1955 _position = NewPos;
1832 fx = nmax; 1956 d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z);
1833 if (fx < nmin) 1957 _orientation = newrot;
1834 fx = nmin; 1958 quat.X = newrot.X;
1835 if (fy > nmax) 1959 quat.Y = newrot.Y;
1836 fy = nmax; 1960 quat.Z = newrot.Z;
1837 if (fy < nmin) 1961 quat.W = newrot.W;
1838 fy = nmin; 1962 d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat);
1839 d.BodyAddForce(Body, fx, fy, fz); 1963
1840//Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); 1964 mat = d.GeomGetOffsetRotation(prim_geom);
1841 } 1965 d.MassRotate(ref tmpdmass, ref mat);
1842 } 1966
1843 } 1967 thispos = d.GeomGetOffsetPosition(prim_geom);
1844 else 1968 d.MassTranslate(ref tmpdmass,
1845 { // is not physical, or is not a body or is selected 1969 thispos.X,
1846 // _zeroPosition = d.BodyGetPosition(Body); 1970 thispos.Y,
1847 return; 1971 thispos.Z);
1848//Console.WriteLine("Nothing " + Name); 1972
1849 1973 d.MassAdd(ref objdmass, ref tmpdmass);
1974
1975 // fix all positions
1976 IntPtr g = d.BodyGetFirstGeom(Body);
1977 while (g != IntPtr.Zero)
1978 {
1979 thispos = d.GeomGetOffsetPosition(g);
1980 thispos.X -= objdmass.c.X;
1981 thispos.Y -= objdmass.c.Y;
1982 thispos.Z -= objdmass.c.Z;
1983 d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
1984 g = d.dBodyGetNextGeom(g);
1850 } 1985 }
1986 d.BodyVectorToWorld(Body,objdmass.c.X, objdmass.c.Y, objdmass.c.Z,out thispos);
1987
1988 d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
1989 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
1990 d.BodySetMass(Body, ref objdmass);
1991 _mass = objdmass.mass;
1851 } 1992 }
1852 1993
1853 private void rotate() 1994
1995
1996 private void FixInertia(Vector3 NewPos)
1854 { 1997 {
1855 d.Quaternion myrot = new d.Quaternion(); 1998 d.Matrix3 primmat = new d.Matrix3();
1856 myrot.X = _orientation.X; 1999 d.Mass tmpdmass = new d.Mass { };
1857 myrot.Y = _orientation.Y; 2000 d.Mass objdmass = new d.Mass { };
1858 myrot.Z = _orientation.Z; 2001 d.Mass primmass = new d.Mass { };
1859 myrot.W = _orientation.W; 2002
1860 if (Body != IntPtr.Zero) 2003 d.Vector3 dobjpos;
1861 { 2004 d.Vector3 thispos;
1862 // KF: If this is a root prim do BodySet 2005
1863 d.BodySetQuaternion(Body, ref myrot); 2006 d.BodyGetMass(Body, out objdmass);
1864 if (IsPhysical) 2007
1865 { 2008 // get prim own inertia in its local frame
1866 if (!m_angularlock.ApproxEquals(Vector3.One, 0f)) 2009 primmass = primdMass;
1867 createAMotor(m_angularlock); 2010 // transform to object frame
1868 } 2011 primmat = d.GeomGetOffsetRotation(prim_geom);
1869 } 2012 d.MassRotate(ref primmass, ref primmat);
1870 else 2013
2014 tmpdmass = primmass;
2015
2016 thispos = d.GeomGetOffsetPosition(prim_geom);
2017 d.MassTranslate(ref tmpdmass,
2018 thispos.X,
2019 thispos.Y,
2020 thispos.Z);
2021
2022 // subtract current prim inertia from object
2023 DMassSubPartFromObj(ref tmpdmass, ref objdmass);
2024
2025 // update to new position
2026 _position = NewPos;
2027 d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z);
2028
2029 thispos = d.GeomGetOffsetPosition(prim_geom);
2030 d.MassTranslate(ref primmass,
2031 thispos.X,
2032 thispos.Y,
2033 thispos.Z);
2034
2035 d.MassAdd(ref objdmass, ref primmass);
2036
2037 // fix all positions
2038 IntPtr g = d.BodyGetFirstGeom(Body);
2039 while (g != IntPtr.Zero)
1871 { 2040 {
1872 // daughter prim, do Geom set 2041 thispos = d.GeomGetOffsetPosition(g);
1873 d.GeomSetQuaternion(prim_geom, ref myrot); 2042 thispos.X -= objdmass.c.X;
2043 thispos.Y -= objdmass.c.Y;
2044 thispos.Z -= objdmass.c.Z;
2045 d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
2046 g = d.dBodyGetNextGeom(g);
1874 } 2047 }
1875 2048
1876 resetCollisionAccounting(); 2049 d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos);
1877 m_taintrot = _orientation; 2050
2051 // get current object position and rotation
2052 dobjpos = d.BodyGetPosition(Body);
2053
2054 d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
2055 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
2056 d.BodySetMass(Body, ref objdmass);
2057 _mass = objdmass.mass;
1878 } 2058 }
1879 2059
1880 private void resetCollisionAccounting() 2060 private void FixInertia(Quaternion newrot)
1881 { 2061 {
1882 m_collisionscore = 0; 2062 d.Matrix3 mat = new d.Matrix3();
1883 m_interpenetrationcount = 0; 2063 d.Quaternion quat = new d.Quaternion();
1884 m_disabled = false; 2064
2065 d.Mass tmpdmass = new d.Mass { };
2066 d.Mass objdmass = new d.Mass { };
2067 d.Vector3 dobjpos;
2068 d.Vector3 thispos;
2069
2070 d.BodyGetMass(Body, out objdmass);
2071
2072 // get prim own inertia in its local frame
2073 tmpdmass = primdMass;
2074 mat = d.GeomGetOffsetRotation(prim_geom);
2075 d.MassRotate(ref tmpdmass, ref mat);
2076 // transform to object frame
2077 thispos = d.GeomGetOffsetPosition(prim_geom);
2078 d.MassTranslate(ref tmpdmass,
2079 thispos.X,
2080 thispos.Y,
2081 thispos.Z);
2082
2083 // subtract current prim inertia from object
2084 DMassSubPartFromObj(ref tmpdmass, ref objdmass);
2085
2086 // update to new orientation
2087 _orientation = newrot;
2088 quat.X = newrot.X;
2089 quat.Y = newrot.Y;
2090 quat.Z = newrot.Z;
2091 quat.W = newrot.W;
2092 d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat);
2093
2094 tmpdmass = primdMass;
2095 mat = d.GeomGetOffsetRotation(prim_geom);
2096 d.MassRotate(ref tmpdmass, ref mat);
2097 d.MassTranslate(ref tmpdmass,
2098 thispos.X,
2099 thispos.Y,
2100 thispos.Z);
2101
2102 d.MassAdd(ref objdmass, ref tmpdmass);
2103
2104 // fix all positions
2105 IntPtr g = d.BodyGetFirstGeom(Body);
2106 while (g != IntPtr.Zero)
2107 {
2108 thispos = d.GeomGetOffsetPosition(g);
2109 thispos.X -= objdmass.c.X;
2110 thispos.Y -= objdmass.c.Y;
2111 thispos.Z -= objdmass.c.Z;
2112 d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
2113 g = d.dBodyGetNextGeom(g);
2114 }
2115
2116 d.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos);
2117 // get current object position and rotation
2118 dobjpos = d.BodyGetPosition(Body);
2119
2120 d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
2121 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
2122 d.BodySetMass(Body, ref objdmass);
2123 _mass = objdmass.mass;
1885 } 2124 }
1886 2125
1887 /// <summary> 2126
1888 /// Change prim in response to a disable taint. 2127 #region Mass Calculation
1889 /// </summary> 2128
1890 private void changedisable() 2129 private void UpdatePrimBodyData()
1891 { 2130 {
1892 m_disabled = true; 2131 primMass = m_density * primVolume;
1893 if (Body != IntPtr.Zero) 2132
2133 if (primMass <= 0)
2134 primMass = 0.0001f;//ckrinke: Mass must be greater then zero.
2135 if (primMass > _parent_scene.maximumMassObject)
2136 primMass = _parent_scene.maximumMassObject;
2137
2138 _mass = primMass; // just in case
2139
2140 d.MassSetBoxTotal(out primdMass, primMass, m_OBB.X, m_OBB.Y, m_OBB.Z);
2141
2142 d.MassTranslate(ref primdMass,
2143 m_OBBOffset.X,
2144 m_OBBOffset.Y,
2145 m_OBBOffset.Z);
2146
2147 primOOBradiusSQ = m_OBB.LengthSquared();
2148
2149 if (_triMeshData != IntPtr.Zero)
1894 { 2150 {
1895 d.BodyDisable(Body); 2151 float pc = m_physCost;
1896 Body = IntPtr.Zero; 2152 float psf = primOOBradiusSQ;
2153 psf *= 1.33f * .2f;
2154 pc *= psf;
2155 if (pc < 0.1f)
2156 pc = 0.1f;
2157
2158 m_physCost = pc;
1897 } 2159 }
2160 else
2161 m_physCost = 0.1f;
1898 2162
1899 m_taintdisable = false; 2163 m_streamCost = 1.0f;
1900 } 2164 }
1901 2165
2166 #endregion
2167
2168
1902 /// <summary> 2169 /// <summary>
1903 /// Change prim in response to a physics status taint 2170 /// Add a child prim to this parent prim.
1904 /// </summary> 2171 /// </summary>
1905 private void changePhysicsStatus() 2172 /// <param name="prim">Child prim</param>
2173 // I'm the parent
2174 // prim is the child
2175 public void ParentPrim(OdePrim prim)
1906 { 2176 {
1907 if (IsPhysical) 2177 //Console.WriteLine("ParentPrim " + m_primName);
2178 if (this.m_localID != prim.m_localID)
1908 { 2179 {
1909 if (Body == IntPtr.Zero) 2180 DestroyBody(); // for now we need to rebuil entire object on link change
2181
2182 lock (childrenPrim)
1910 { 2183 {
1911 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) 2184 // adopt the prim
1912 { 2185 if (!childrenPrim.Contains(prim))
1913 changeshape(); 2186 childrenPrim.Add(prim);
1914 } 2187
1915 else 2188 // see if this prim has kids and adopt them also
2189 // should not happen for now
2190 foreach (OdePrim prm in prim.childrenPrim)
1916 { 2191 {
1917 enableBody(); 2192 if (!childrenPrim.Contains(prm))
2193 {
2194 if (prm.Body != IntPtr.Zero)
2195 {
2196 if (prm.prim_geom != IntPtr.Zero)
2197 d.GeomSetBody(prm.prim_geom, IntPtr.Zero);
2198 if (prm.Body != prim.Body)
2199 prm.DestroyBody(); // don't loose bodies around
2200 prm.Body = IntPtr.Zero;
2201 }
2202
2203 childrenPrim.Add(prm);
2204 prm._parent = this;
2205 }
1918 } 2206 }
1919 } 2207 }
2208 //Remove old children from the prim
2209 prim.childrenPrim.Clear();
2210
2211 if (prim.Body != IntPtr.Zero)
2212 {
2213 if (prim.prim_geom != IntPtr.Zero)
2214 d.GeomSetBody(prim.prim_geom, IntPtr.Zero);
2215 prim.DestroyBody(); // don't loose bodies around
2216 prim.Body = IntPtr.Zero;
2217 }
2218
2219 prim.childPrim = true;
2220 prim._parent = this;
2221
2222 MakeBody(); // full nasty reconstruction
1920 } 2223 }
1921 else 2224 }
2225
2226 private void UpdateChildsfromgeom()
2227 {
2228 if (childrenPrim.Count > 0)
1922 { 2229 {
1923 if (Body != IntPtr.Zero) 2230 foreach (OdePrim prm in childrenPrim)
1924 { 2231 prm.UpdateDataFromGeom();
1925 if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim) 2232 }
1926 { 2233 }
1927 RemoveGeom();
1928 2234
1929//Console.WriteLine("changePhysicsStatus for " + Name); 2235 private void UpdateDataFromGeom()
1930 changeadd(); 2236 {
1931 } 2237 if (prim_geom != IntPtr.Zero)
2238 {
2239 d.Quaternion qtmp;
2240 d.GeomCopyQuaternion(prim_geom, out qtmp);
2241 _orientation.X = qtmp.X;
2242 _orientation.Y = qtmp.Y;
2243 _orientation.Z = qtmp.Z;
2244 _orientation.W = qtmp.W;
2245/*
2246// Debug
2247 float qlen = _orientation.Length();
2248 if (qlen > 1.01f || qlen < 0.99)
2249 m_log.WarnFormat("[PHYSICS]: Got nonnorm quaternion from geom in Object {0} norm {1}", Name, qlen);
2250//
2251*/
2252 _orientation.Normalize();
2253
2254 d.Vector3 lpos = d.GeomGetPosition(prim_geom);
2255 _position.X = lpos.X;
2256 _position.Y = lpos.Y;
2257 _position.Z = lpos.Z;
2258 }
2259 }
2260
2261 private void ChildDelink(OdePrim odePrim, bool remakebodies)
2262 {
2263 // Okay, we have a delinked child.. destroy all body and remake
2264 if (odePrim != this && !childrenPrim.Contains(odePrim))
2265 return;
2266
2267 DestroyBody();
1932 2268
1933 if (childPrim) 2269 if (odePrim == this) // delinking the root prim
2270 {
2271 OdePrim newroot = null;
2272 lock (childrenPrim)
2273 {
2274 if (childrenPrim.Count > 0)
1934 { 2275 {
1935 if (_parent != null) 2276 newroot = childrenPrim[0];
2277 childrenPrim.RemoveAt(0);
2278 foreach (OdePrim prm in childrenPrim)
1936 { 2279 {
1937 OdePrim parent = (OdePrim)_parent; 2280 newroot.childrenPrim.Add(prm);
1938 parent.ChildDelink(this);
1939 } 2281 }
2282 childrenPrim.Clear();
1940 } 2283 }
1941 else 2284 if (newroot != null)
1942 { 2285 {
1943 disableBody(); 2286 newroot.childPrim = false;
2287 newroot._parent = null;
2288 if (remakebodies)
2289 newroot.MakeBody();
1944 } 2290 }
1945 } 2291 }
1946 } 2292 }
1947 2293
1948 changeSelectedStatus(); 2294 else
1949 2295 {
1950 resetCollisionAccounting(); 2296 lock (childrenPrim)
1951 m_taintPhysics = IsPhysical; 2297 {
2298 childrenPrim.Remove(odePrim);
2299 odePrim.childPrim = false;
2300 odePrim._parent = null;
2301 // odePrim.UpdateDataFromGeom();
2302 if (remakebodies)
2303 odePrim.MakeBody();
2304 }
2305 }
2306 if (remakebodies)
2307 MakeBody();
1952 } 2308 }
1953 2309
1954 /// <summary> 2310 protected void ChildRemove(OdePrim odePrim, bool reMakeBody)
1955 /// Change prim in response to a size taint.
1956 /// </summary>
1957 private void changesize()
1958 { 2311 {
1959#if SPAM 2312 // Okay, we have a delinked child.. destroy all body and remake
1960 m_log.DebugFormat("[ODE PRIM]: Called changesize"); 2313 if (odePrim != this && !childrenPrim.Contains(odePrim))
1961#endif 2314 return;
1962 2315
1963 if (_size.X <= 0) _size.X = 0.01f; 2316 DestroyBody();
1964 if (_size.Y <= 0) _size.Y = 0.01f;
1965 if (_size.Z <= 0) _size.Z = 0.01f;
1966 2317
1967 //kill body to rebuild 2318 if (odePrim == this)
1968 if (IsPhysical && Body != IntPtr.Zero)
1969 { 2319 {
1970 if (childPrim) 2320 OdePrim newroot = null;
2321 lock (childrenPrim)
1971 { 2322 {
1972 if (_parent != null) 2323 if (childrenPrim.Count > 0)
1973 { 2324 {
1974 OdePrim parent = (OdePrim)_parent; 2325 newroot = childrenPrim[0];
1975 parent.ChildDelink(this); 2326 childrenPrim.RemoveAt(0);
2327 foreach (OdePrim prm in childrenPrim)
2328 {
2329 newroot.childrenPrim.Add(prm);
2330 }
2331 childrenPrim.Clear();
2332 }
2333 if (newroot != null)
2334 {
2335 newroot.childPrim = false;
2336 newroot._parent = null;
2337 newroot.MakeBody();
1976 } 2338 }
1977 } 2339 }
1978 else 2340 if (reMakeBody)
1979 { 2341 MakeBody();
1980 disableBody(); 2342 return;
1981 }
1982 } 2343 }
1983 2344 else
1984 if (d.SpaceQuery(m_targetSpace, prim_geom))
1985 { 2345 {
1986// _parent_scene.waitForSpaceUnlock(m_targetSpace); 2346 lock (childrenPrim)
1987 d.SpaceRemove(m_targetSpace, prim_geom); 2347 {
2348 childrenPrim.Remove(odePrim);
2349 odePrim.childPrim = false;
2350 odePrim._parent = null;
2351 if (reMakeBody)
2352 odePrim.MakeBody();
2353 }
1988 } 2354 }
2355 MakeBody();
2356 }
1989 2357
1990 RemoveGeom(); 2358 #region changes
1991
1992 // we don't need to do space calculation because the client sends a position update also.
1993 2359
1994 IMesh mesh = null; 2360 private void changeadd()
2361 {
2362 }
1995 2363
1996 // Construction of new prim 2364 private void changeAngularLock(Vector3 newLock)
1997 if (_parent_scene.needsMeshing(_pbs)) 2365 {
2366 // do we have a Physical object?
2367 if (Body != IntPtr.Zero)
1998 { 2368 {
1999 float meshlod = _parent_scene.meshSculptLOD; 2369 //Check that we have a Parent
2000 2370 //If we have a parent then we're not authorative here
2001 if (IsPhysical) 2371 if (_parent == null)
2002 meshlod = _parent_scene.MeshSculptphysicalLOD;
2003 // Don't need to re-enable body.. it's done in SetMesh
2004
2005 if (_parent_scene.needsMeshing(_pbs))
2006 { 2372 {
2007 mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical); 2373 if (!newLock.ApproxEquals(Vector3.One, 0f))
2008 if (mesh == null) 2374 {
2009 CheckMeshAsset(); 2375 createAMotor(newLock);
2376 }
2377 else
2378 {
2379 if (Amotor != IntPtr.Zero)
2380 {
2381 d.JointDestroy(Amotor);
2382 Amotor = IntPtr.Zero;
2383 }
2384 }
2010 } 2385 }
2011
2012 } 2386 }
2387 // Store this for later in case we get turned into a separate body
2388 m_angularlock = newLock;
2389 }
2013 2390
2014 CreateGeom(m_targetSpace, mesh); 2391 private void changeLink(OdePrim NewParent)
2015 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); 2392 {
2016 d.Quaternion myrot = new d.Quaternion(); 2393 if (_parent == null && NewParent != null)
2017 myrot.X = _orientation.X;
2018 myrot.Y = _orientation.Y;
2019 myrot.Z = _orientation.Z;
2020 myrot.W = _orientation.W;
2021 d.GeomSetQuaternion(prim_geom, ref myrot);
2022
2023 //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
2024 if (IsPhysical && Body == IntPtr.Zero && !childPrim)
2025 { 2394 {
2026 // Re creates body on size. 2395 NewParent.ParentPrim(this);
2027 // EnableBody also does setMass()
2028 enableBody();
2029 d.BodyEnable(Body);
2030 } 2396 }
2031 2397 else if (_parent != null)
2032 changeSelectedStatus();
2033
2034 if (childPrim)
2035 { 2398 {
2036 if (_parent is OdePrim) 2399 if (_parent is OdePrim)
2037 { 2400 {
2038 OdePrim parent = (OdePrim)_parent; 2401 if (NewParent != _parent)
2039 parent.ChildSetGeom(this); 2402 {
2403 (_parent as OdePrim).ChildDelink(this, false); // for now...
2404 childPrim = false;
2405
2406 if (NewParent != null)
2407 {
2408 NewParent.ParentPrim(this);
2409 }
2410 }
2040 } 2411 }
2041 } 2412 }
2042 resetCollisionAccounting(); 2413 _parent = NewParent;
2043 m_taintsize = _size;
2044 } 2414 }
2045 2415
2046 /// <summary>
2047 /// Change prim in response to a float on water taint.
2048 /// </summary>
2049 /// <param name="timestep"></param>
2050 private void changefloatonwater()
2051 {
2052 m_collidesWater = m_taintCollidesWater;
2053 2416
2054 if (m_collidesWater) 2417 private void Stop()
2418 {
2419 if (!childPrim)
2055 { 2420 {
2056 m_collisionFlags |= CollisionCategories.Water; 2421 m_force = Vector3.Zero;
2422 m_forceacc = Vector3.Zero;
2423 m_angularForceacc = Vector3.Zero;
2424 _torque = Vector3.Zero;
2425 _velocity = Vector3.Zero;
2426 _acceleration = Vector3.Zero;
2427 m_rotationalVelocity = Vector3.Zero;
2428 _target_velocity = Vector3.Zero;
2429 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
2430 m_vehicle.Stop();
2431
2432 _zeroFlag = false;
2433 base.RequestPhysicsterseUpdate();
2057 } 2434 }
2058 else 2435
2436 if (Body != IntPtr.Zero)
2059 { 2437 {
2060 m_collisionFlags &= ~CollisionCategories.Water; 2438 d.BodySetForce(Body, 0f, 0f, 0f);
2439 d.BodySetTorque(Body, 0f, 0f, 0f);
2440 d.BodySetLinearVel(Body, 0f, 0f, 0f);
2441 d.BodySetAngularVel(Body, 0f, 0f, 0f);
2061 } 2442 }
2062
2063 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
2064 } 2443 }
2065 2444
2066 /// <summary> 2445 private void changePhantomStatus(bool newval)
2067 /// Change prim in response to a shape taint.
2068 /// </summary>
2069 private void changeshape()
2070 { 2446 {
2071 m_taintshape = false; 2447 m_isphantom = newval;
2072
2073 // Cleanup of old prim geometry and Bodies
2074 if (IsPhysical && Body != IntPtr.Zero)
2075 {
2076 if (childPrim)
2077 {
2078 if (_parent != null)
2079 {
2080 OdePrim parent = (OdePrim)_parent;
2081 parent.ChildDelink(this);
2082 }
2083 }
2084 else
2085 {
2086 disableBody();
2087 }
2088 }
2089 2448
2090 RemoveGeom(); 2449 UpdateCollisionCatFlags();
2091 2450 ApplyCollisionCatFlags();
2092 // we don't need to do space calculation because the client sends a position update also. 2451 }
2093 if (_size.X <= 0) _size.X = 0.01f;
2094 if (_size.Y <= 0) _size.Y = 0.01f;
2095 if (_size.Z <= 0) _size.Z = 0.01f;
2096 // Construction of new prim
2097 2452
2098 IMesh mesh = null; 2453/* not in use
2454 internal void ChildSelectedChange(bool childSelect)
2455 {
2456 if(childPrim)
2457 return;
2099 2458
2459 if (childSelect == m_isSelected)
2460 return;
2100 2461
2101 if (_parent_scene.needsMeshing(_pbs)) 2462 if (childSelect)
2102 { 2463 {
2103 // Don't need to re-enable body.. it's done in CreateMesh 2464 DoSelectedStatus(true);
2104 float meshlod = _parent_scene.meshSculptLOD;
2105
2106 if (IsPhysical)
2107 meshlod = _parent_scene.MeshSculptphysicalLOD;
2108
2109 // createmesh returns null when it doesn't mesh.
2110 mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical);
2111 if (mesh == null)
2112 CheckMeshAsset();
2113 } 2465 }
2114 2466
2115 CreateGeom(m_targetSpace, mesh); 2467 else
2116 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2117 d.Quaternion myrot = new d.Quaternion();
2118 //myrot.W = _orientation.w;
2119 myrot.W = _orientation.W;
2120 myrot.X = _orientation.X;
2121 myrot.Y = _orientation.Y;
2122 myrot.Z = _orientation.Z;
2123 d.GeomSetQuaternion(prim_geom, ref myrot);
2124
2125 //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
2126 if (IsPhysical && Body == IntPtr.Zero)
2127 { 2468 {
2128 // Re creates body on size. 2469 foreach (OdePrim prm in childrenPrim)
2129 // EnableBody also does setMass()
2130 enableBody();
2131 if (Body != IntPtr.Zero)
2132 { 2470 {
2133 d.BodyEnable(Body); 2471 if (prm.m_isSelected)
2472 return;
2134 } 2473 }
2474 DoSelectedStatus(false);
2135 } 2475 }
2476 }
2477*/
2478 private void changeSelectedStatus(bool newval)
2479 {
2480 if (m_lastdoneSelected == newval)
2481 return;
2136 2482
2137 changeSelectedStatus(); 2483 m_lastdoneSelected = newval;
2484 DoSelectedStatus(newval);
2485 }
2138 2486
2139 if (childPrim) 2487 private void CheckDelaySelect()
2488 {
2489 if (m_delaySelect)
2140 { 2490 {
2141 if (_parent is OdePrim) 2491 DoSelectedStatus(m_isSelected);
2142 {
2143 OdePrim parent = (OdePrim)_parent;
2144 parent.ChildSetGeom(this);
2145 }
2146 } 2492 }
2147
2148 resetCollisionAccounting();
2149// m_taintshape = false;
2150 } 2493 }
2151 2494
2152 /// <summary> 2495 private void DoSelectedStatus(bool newval)
2153 /// Change prim in response to an add force taint.
2154 /// </summary>
2155 private void changeAddForce()
2156 { 2496 {
2157 if (!m_isSelected) 2497 m_isSelected = newval;
2498 Stop();
2499
2500 if (newval)
2158 { 2501 {
2159 lock (m_forcelist) 2502 if (!childPrim && Body != IntPtr.Zero)
2503 d.BodyDisable(Body);
2504
2505 if (m_delaySelect || m_isphysical)
2160 { 2506 {
2161 //m_log.Info("[PHYSICS]: dequeing forcelist"); 2507 m_collisionCategories = CollisionCategories.Selected;
2162 if (IsPhysical) 2508 m_collisionFlags = 0;
2509
2510 if (!childPrim)
2163 { 2511 {
2164 Vector3 iforce = Vector3.Zero; 2512 foreach (OdePrim prm in childrenPrim)
2165 int i = 0;
2166 try
2167 { 2513 {
2168 for (i = 0; i < m_forcelist.Count; i++) 2514 prm.m_collisionCategories = m_collisionCategories;
2515 prm.m_collisionFlags = m_collisionFlags;
2516
2517 if (prm.prim_geom != null)
2169 { 2518 {
2170 2519
2171 iforce = iforce + (m_forcelist[i] * 100); 2520 if (prm.m_NoColide)
2521 {
2522 d.GeomSetCategoryBits(prm.prim_geom, 0);
2523 d.GeomSetCollideBits(prm.prim_geom, 0);
2524 }
2525 else
2526 {
2527 d.GeomSetCategoryBits(prm.prim_geom, (uint)m_collisionCategories);
2528 d.GeomSetCollideBits(prm.prim_geom, (uint)m_collisionFlags);
2529 }
2172 } 2530 }
2531 prm.m_delaySelect = false;
2173 } 2532 }
2174 catch (IndexOutOfRangeException) 2533 }
2534// else if (_parent != null)
2535// ((OdePrim)_parent).ChildSelectedChange(true);
2536
2537
2538 if (prim_geom != null)
2539 {
2540 if (m_NoColide)
2175 { 2541 {
2176 m_forcelist = new List<Vector3>(); 2542 d.GeomSetCategoryBits(prim_geom, 0);
2177 m_collisionscore = 0; 2543 d.GeomSetCollideBits(prim_geom, 0);
2178 m_interpenetrationcount = 0; 2544 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
2179 m_taintforce = false; 2545 {
2180 return; 2546 d.GeomSetCategoryBits(collide_geom, 0);
2547 d.GeomSetCollideBits(collide_geom, 0);
2548 }
2549
2181 } 2550 }
2182 catch (ArgumentOutOfRangeException) 2551 else
2183 { 2552 {
2184 m_forcelist = new List<Vector3>(); 2553 d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
2185 m_collisionscore = 0; 2554 d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
2186 m_interpenetrationcount = 0; 2555 if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
2187 m_taintforce = false; 2556 {
2188 return; 2557 d.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories);
2558 d.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags);
2559 }
2189 } 2560 }
2190 d.BodyEnable(Body);
2191 d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z);
2192 } 2561 }
2193 m_forcelist.Clear(); 2562
2563 m_delaySelect = false;
2564 }
2565 else if(!m_isphysical)
2566 {
2567 m_delaySelect = true;
2568 }
2569 }
2570 else
2571 {
2572 if (!childPrim)
2573 {
2574 if (Body != IntPtr.Zero && !m_disabled)
2575 d.BodyEnable(Body);
2194 } 2576 }
2577// else if (_parent != null)
2578// ((OdePrim)_parent).ChildSelectedChange(false);
2195 2579
2196 m_collisionscore = 0; 2580 UpdateCollisionCatFlags();
2197 m_interpenetrationcount = 0; 2581 ApplyCollisionCatFlags();
2582
2583 m_delaySelect = false;
2198 } 2584 }
2199 2585
2200 m_taintforce = false; 2586 resetCollisionAccounting();
2201 } 2587 }
2202 2588
2203 /// <summary> 2589 private void changePosition(Vector3 newPos)
2204 /// Change prim in response to a torque taint.
2205 /// </summary>
2206 private void changeSetTorque()
2207 { 2590 {
2208 if (!m_isSelected) 2591 CheckDelaySelect();
2592 if (m_isphysical)
2209 { 2593 {
2210 if (IsPhysical && Body != IntPtr.Zero) 2594 if (childPrim) // inertia is messed, must rebuild
2595 {
2596 if (m_building)
2597 {
2598 _position = newPos;
2599 }
2600
2601 else if (m_forcePosOrRotation && _position != newPos && Body != IntPtr.Zero)
2602 {
2603 FixInertia(newPos);
2604 if (!d.BodyIsEnabled(Body))
2605 d.BodyEnable(Body);
2606 }
2607 }
2608 else
2211 { 2609 {
2212 d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z); 2610 if (_position != newPos)
2611 {
2612 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2613 _position = newPos;
2614 }
2615 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2616 d.BodyEnable(Body);
2213 } 2617 }
2214 } 2618 }
2619 else
2620 {
2621 if (prim_geom != IntPtr.Zero)
2622 {
2623 if (newPos != _position)
2624 {
2625 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2626 _position = newPos;
2215 2627
2216 m_taintTorque = Vector3.Zero; 2628 m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace);
2629 }
2630 }
2631 }
2632 givefakepos--;
2633 if (givefakepos < 0)
2634 givefakepos = 0;
2635// changeSelectedStatus();
2636 resetCollisionAccounting();
2217 } 2637 }
2218 2638
2219 /// <summary> 2639 private void changeOrientation(Quaternion newOri)
2220 /// Change prim in response to an angular force taint.
2221 /// </summary>
2222 private void changeAddAngularForce()
2223 { 2640 {
2224 if (!m_isSelected) 2641 CheckDelaySelect();
2642 if (m_isphysical)
2225 { 2643 {
2226 lock (m_angularforcelist) 2644 if (childPrim) // inertia is messed, must rebuild
2227 { 2645 {
2228 //m_log.Info("[PHYSICS]: dequeing forcelist"); 2646 if (m_building)
2229 if (IsPhysical)
2230 { 2647 {
2231 Vector3 iforce = Vector3.Zero; 2648 _orientation = newOri;
2232 for (int i = 0; i < m_angularforcelist.Count; i++) 2649 }
2233 { 2650/*
2234 iforce = iforce + (m_angularforcelist[i] * 100); 2651 else if (m_forcePosOrRotation && _orientation != newOri && Body != IntPtr.Zero)
2235 } 2652 {
2653 FixInertia(_position, newOri);
2654 if (!d.BodyIsEnabled(Body))
2655 d.BodyEnable(Body);
2656 }
2657*/
2658 }
2659 else
2660 {
2661 if (newOri != _orientation)
2662 {
2663 d.Quaternion myrot = new d.Quaternion();
2664 myrot.X = newOri.X;
2665 myrot.Y = newOri.Y;
2666 myrot.Z = newOri.Z;
2667 myrot.W = newOri.W;
2668 d.GeomSetQuaternion(prim_geom, ref myrot);
2669 _orientation = newOri;
2670 if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
2671 createAMotor(m_angularlock);
2672 }
2673 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2236 d.BodyEnable(Body); 2674 d.BodyEnable(Body);
2237 d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z); 2675 }
2238 2676 }
2677 else
2678 {
2679 if (prim_geom != IntPtr.Zero)
2680 {
2681 if (newOri != _orientation)
2682 {
2683 d.Quaternion myrot = new d.Quaternion();
2684 myrot.X = newOri.X;
2685 myrot.Y = newOri.Y;
2686 myrot.Z = newOri.Z;
2687 myrot.W = newOri.W;
2688 d.GeomSetQuaternion(prim_geom, ref myrot);
2689 _orientation = newOri;
2239 } 2690 }
2240 m_angularforcelist.Clear();
2241 } 2691 }
2242
2243 m_collisionscore = 0;
2244 m_interpenetrationcount = 0;
2245 } 2692 }
2246 2693 givefakeori--;
2247 m_taintaddangularforce = false; 2694 if (givefakeori < 0)
2695 givefakeori = 0;
2696 resetCollisionAccounting();
2248 } 2697 }
2249 2698
2250 /// <summary> 2699 private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri)
2251 /// Change prim in response to a velocity taint.
2252 /// </summary>
2253 private void changevelocity()
2254 { 2700 {
2255 if (!m_isSelected) 2701 CheckDelaySelect();
2702 if (m_isphysical)
2256 { 2703 {
2257 // Not sure exactly why this sleep is here, but from experimentation it appears to stop an avatar 2704 if (childPrim && m_building) // inertia is messed, must rebuild
2258 // walking through a default rez size prim if it keeps kicking it around - justincc.
2259 Thread.Sleep(20);
2260
2261 if (IsPhysical)
2262 { 2705 {
2263 if (Body != IntPtr.Zero) 2706 _position = newPos;
2707 _orientation = newOri;
2708 }
2709 else
2710 {
2711 if (newOri != _orientation)
2712 {
2713 d.Quaternion myrot = new d.Quaternion();
2714 myrot.X = newOri.X;
2715 myrot.Y = newOri.Y;
2716 myrot.Z = newOri.Z;
2717 myrot.W = newOri.W;
2718 d.GeomSetQuaternion(prim_geom, ref myrot);
2719 _orientation = newOri;
2720 if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
2721 createAMotor(m_angularlock);
2722 }
2723 if (_position != newPos)
2264 { 2724 {
2265 d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z); 2725 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2726 _position = newPos;
2266 } 2727 }
2728 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2729 d.BodyEnable(Body);
2267 } 2730 }
2268
2269 //resetCollisionAccounting();
2270 } 2731 }
2732 else
2733 {
2734 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
2735 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
2271 2736
2272 m_taintVelocity = Vector3.Zero; 2737 if (prim_geom != IntPtr.Zero)
2273 } 2738 {
2739 if (newOri != _orientation)
2740 {
2741 d.Quaternion myrot = new d.Quaternion();
2742 myrot.X = newOri.X;
2743 myrot.Y = newOri.Y;
2744 myrot.Z = newOri.Z;
2745 myrot.W = newOri.W;
2746 d.GeomSetQuaternion(prim_geom, ref myrot);
2747 _orientation = newOri;
2748 }
2274 2749
2275 internal void setPrimForRemoval() 2750 if (newPos != _position)
2276 { 2751 {
2277 m_taintremove = true; 2752 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2278 } 2753 _position = newPos;
2279 2754
2280 public override bool Flying 2755 m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace);
2281 { 2756 }
2282 // no flying prims for you 2757 }
2283 get { return false; } 2758 }
2284 set { } 2759 givefakepos--;
2760 if (givefakepos < 0)
2761 givefakepos = 0;
2762 givefakeori--;
2763 if (givefakeori < 0)
2764 givefakeori = 0;
2765 resetCollisionAccounting();
2285 } 2766 }
2286 2767
2287 public override bool IsColliding 2768 private void changeDisable(bool disable)
2288 { 2769 {
2289 get { return iscolliding; } 2770 if (disable)
2290 set { iscolliding = value; } 2771 {
2772 if (!m_disabled)
2773 disableBodySoft();
2774 }
2775 else
2776 {
2777 if (m_disabled)
2778 enableBodySoft();
2779 }
2291 } 2780 }
2292 2781
2293 public override bool CollidingGround 2782 private void changePhysicsStatus(bool NewStatus)
2294 { 2783 {
2295 get { return false; } 2784 CheckDelaySelect();
2296 set { return; }
2297 }
2298 2785
2299 public override bool CollidingObj 2786 m_isphysical = NewStatus;
2300 { 2787
2301 get { return false; } 2788 if (!childPrim)
2302 set { return; } 2789 {
2790 if (NewStatus)
2791 {
2792 if (Body == IntPtr.Zero)
2793 MakeBody();
2794 }
2795 else
2796 {
2797 if (Body != IntPtr.Zero)
2798 {
2799 DestroyBody();
2800 }
2801 Stop();
2802 }
2803 }
2804
2805 resetCollisionAccounting();
2303 } 2806 }
2304 2807
2305 public override bool ThrottleUpdates 2808 private void changeSize(Vector3 newSize)
2306 { 2809 {
2307 get { return m_throttleUpdates; }
2308 set { m_throttleUpdates = value; }
2309 } 2810 }
2310 2811
2311 public override bool Stopped 2812 private void changeShape(PrimitiveBaseShape newShape)
2312 { 2813 {
2313 get { return _zeroFlag; }
2314 } 2814 }
2315 2815
2316 public override Vector3 Position 2816 private void changeAddPhysRep(ODEPhysRepData repData)
2317 { 2817 {
2318 get { return _position; } 2818 _size = repData.size; //??
2819 _pbs = repData.pbs;
2820 m_shapetype = repData.shapetype;
2821
2822 m_mesh = repData.mesh;
2823
2824 m_assetID = repData.assetID;
2825 m_meshState = repData.meshState;
2826
2827 m_hasOBB = repData.hasOBB;
2828 m_OBBOffset = repData.OBBOffset;
2829 m_OBB = repData.OBB;
2319 2830
2320 set { _position = value; 2831 primVolume = repData.volume;
2321 //m_log.Info("[PHYSICS]: " + _position.ToString()); 2832
2833 CreateGeom();
2834
2835 if (prim_geom != IntPtr.Zero)
2836 {
2837 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2838 d.Quaternion myrot = new d.Quaternion();
2839 myrot.X = _orientation.X;
2840 myrot.Y = _orientation.Y;
2841 myrot.Z = _orientation.Z;
2842 myrot.W = _orientation.W;
2843 d.GeomSetQuaternion(prim_geom, ref myrot);
2844 }
2845
2846 if (!m_isphysical)
2847 {
2848 SetInStaticSpace(this);
2849 UpdateCollisionCatFlags();
2850 ApplyCollisionCatFlags();
2851 }
2852 else
2853 MakeBody();
2854
2855 if ((m_meshState & MeshState.NeedMask) != 0)
2856 {
2857 repData.size = _size;
2858 repData.pbs = _pbs;
2859 repData.shapetype = m_shapetype;
2860 _parent_scene.m_meshWorker.RequestMesh(repData);
2322 } 2861 }
2323 } 2862 }
2324 2863
2325 public override Vector3 Size 2864 private void changePhysRepData(ODEPhysRepData repData)
2326 { 2865 {
2327 get { return _size; } 2866 CheckDelaySelect();
2328 set 2867
2868 OdePrim parent = (OdePrim)_parent;
2869
2870 bool chp = childPrim;
2871
2872 if (chp)
2329 { 2873 {
2330 if (value.IsFinite()) 2874 if (parent != null)
2331 { 2875 {
2332 _size = value; 2876 parent.DestroyBody();
2333// m_log.DebugFormat("[PHYSICS]: Set size on {0} to {1}", Name, value);
2334 }
2335 else
2336 {
2337 m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name);
2338 } 2877 }
2339 } 2878 }
2340 } 2879 else
2880 {
2881 DestroyBody();
2882 }
2341 2883
2342 public override float Mass 2884 RemoveGeom();
2343 {
2344 get { return CalculateMass(); }
2345 }
2346 2885
2347 public override Vector3 Force 2886 _size = repData.size;
2348 { 2887 _pbs = repData.pbs;
2349 //get { return Vector3.Zero; } 2888 m_shapetype = repData.shapetype;
2350 get { return m_force; } 2889
2351 set 2890 m_mesh = repData.mesh;
2891
2892 m_assetID = repData.assetID;
2893 m_meshState = repData.meshState;
2894
2895 m_hasOBB = repData.hasOBB;
2896 m_OBBOffset = repData.OBBOffset;
2897 m_OBB = repData.OBB;
2898
2899 primVolume = repData.volume;
2900
2901 CreateGeom();
2902
2903 if (prim_geom != IntPtr.Zero)
2352 { 2904 {
2353 if (value.IsFinite()) 2905 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2906 d.Quaternion myrot = new d.Quaternion();
2907 myrot.X = _orientation.X;
2908 myrot.Y = _orientation.Y;
2909 myrot.Z = _orientation.Z;
2910 myrot.W = _orientation.W;
2911 d.GeomSetQuaternion(prim_geom, ref myrot);
2912 }
2913
2914 if (m_isphysical)
2915 {
2916 if (chp)
2354 { 2917 {
2355 m_force = value; 2918 if (parent != null)
2919 {
2920 parent.MakeBody();
2921 }
2356 } 2922 }
2357 else 2923 else
2358 { 2924 MakeBody();
2359 m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name); 2925 }
2360 } 2926 else
2927 {
2928 SetInStaticSpace(this);
2929 UpdateCollisionCatFlags();
2930 ApplyCollisionCatFlags();
2361 } 2931 }
2362 }
2363
2364 public override int VehicleType
2365 {
2366 get { return (int)m_vehicle.Type; }
2367 set { m_vehicle.ProcessTypeChange((Vehicle)value); }
2368 }
2369 2932
2370 public override void VehicleFloatParam(int param, float value) 2933 resetCollisionAccounting();
2371 {
2372 m_vehicle.ProcessFloatVehicleParam((Vehicle) param, value);
2373 }
2374 2934
2375 public override void VehicleVectorParam(int param, Vector3 value) 2935 if ((m_meshState & MeshState.NeedMask) != 0)
2376 { 2936 {
2377 m_vehicle.ProcessVectorVehicleParam((Vehicle) param, value); 2937 repData.size = _size;
2938 repData.pbs = _pbs;
2939 repData.shapetype = m_shapetype;
2940 _parent_scene.m_meshWorker.RequestMesh(repData);
2941 }
2378 } 2942 }
2379 2943
2380 public override void VehicleRotationParam(int param, Quaternion rotation) 2944 private void changeFloatOnWater(bool newval)
2381 { 2945 {
2382 m_vehicle.ProcessRotationVehicleParam((Vehicle) param, rotation); 2946 m_collidesWater = newval;
2383 }
2384 2947
2385 public override void VehicleFlags(int param, bool remove) 2948 UpdateCollisionCatFlags();
2386 { 2949 ApplyCollisionCatFlags();
2387 m_vehicle.ProcessVehicleFlags(param, remove);
2388 } 2950 }
2389 2951
2390 public override void SetVolumeDetect(int param) 2952 private void changeSetTorque(Vector3 newtorque)
2391 { 2953 {
2392 // We have to lock the scene here so that an entire simulate loop either uses volume detect for all 2954 if (!m_isSelected)
2393 // possible collisions with this prim or for none of them.
2394 lock (_parent_scene.OdeLock)
2395 { 2955 {
2396 m_isVolumeDetect = (param != 0); 2956 if (m_isphysical && Body != IntPtr.Zero)
2957 {
2958 if (m_disabled)
2959 enableBodySoft();
2960 else if (!d.BodyIsEnabled(Body))
2961 d.BodyEnable(Body);
2962
2963 }
2964 _torque = newtorque;
2397 } 2965 }
2398 } 2966 }
2399 2967
2400 public override Vector3 CenterOfMass 2968 private void changeForce(Vector3 force)
2401 { 2969 {
2402 get { return Vector3.Zero; } 2970 m_force = force;
2971 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2972 d.BodyEnable(Body);
2403 } 2973 }
2404 2974
2405 public override Vector3 GeometricCenter 2975 private void changeAddForce(Vector3 theforce)
2406 { 2976 {
2407 get { return Vector3.Zero; } 2977 m_forceacc += theforce;
2978 if (!m_isSelected)
2979 {
2980 lock (this)
2981 {
2982 //m_log.Info("[PHYSICS]: dequeing forcelist");
2983 if (m_isphysical && Body != IntPtr.Zero)
2984 {
2985 if (m_disabled)
2986 enableBodySoft();
2987 else if (!d.BodyIsEnabled(Body))
2988 d.BodyEnable(Body);
2989 }
2990 }
2991 m_collisionscore = 0;
2992 }
2408 } 2993 }
2409 2994
2410 public override PrimitiveBaseShape Shape 2995 // actually angular impulse
2996 private void changeAddAngularImpulse(Vector3 aimpulse)
2411 { 2997 {
2412 set 2998 m_angularForceacc += aimpulse * m_invTimeStep;
2999 if (!m_isSelected)
2413 { 3000 {
2414 _pbs = value; 3001 lock (this)
2415 m_assetFailed = false; 3002 {
2416 m_taintshape = true; 3003 if (m_isphysical && Body != IntPtr.Zero)
3004 {
3005 if (m_disabled)
3006 enableBodySoft();
3007 else if (!d.BodyIsEnabled(Body))
3008 d.BodyEnable(Body);
3009 }
3010 }
3011 m_collisionscore = 0;
2417 } 3012 }
2418 } 3013 }
2419 3014
2420 public override Vector3 Velocity 3015 private void changevelocity(Vector3 newVel)
2421 { 3016 {
2422 get 3017 float len = newVel.LengthSquared();
3018 if (len > 100000.0f) // limit to 100m/s
2423 { 3019 {
2424 // Average previous velocity with the new one so 3020 len = 100.0f / (float)Math.Sqrt(len);
2425 // client object interpolation works a 'little' better 3021 newVel *= len;
2426 if (_zeroFlag)
2427 return Vector3.Zero;
2428
2429 Vector3 returnVelocity = Vector3.Zero;
2430 returnVelocity.X = (m_lastVelocity.X + _velocity.X) * 0.5f; // 0.5f is mathematically equiv to '/ 2'
2431 returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) * 0.5f;
2432 returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) * 0.5f;
2433 return returnVelocity;
2434 } 3022 }
2435 set 3023
3024 if (!m_isSelected)
2436 { 3025 {
2437 if (value.IsFinite()) 3026 if (Body != IntPtr.Zero)
2438 { 3027 {
2439 _velocity = value; 3028 if (m_disabled)
3029 enableBodySoft();
3030 else if (!d.BodyIsEnabled(Body))
3031 d.BodyEnable(Body);
2440 3032
2441 m_taintVelocity = value; 3033 d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z);
2442 _parent_scene.AddPhysicsActorTaint(this);
2443 }
2444 else
2445 {
2446 m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name);
2447 } 3034 }
2448 3035 //resetCollisionAccounting();
2449 } 3036 }
3037 _velocity = newVel;
2450 } 3038 }
2451 3039
2452 public override Vector3 Torque 3040 private void changeangvelocity(Vector3 newAngVel)
2453 { 3041 {
2454 get 3042 float len = newAngVel.LengthSquared();
3043 if (len > 144.0f) // limit to 12rad/s
2455 { 3044 {
2456 if (!IsPhysical || Body == IntPtr.Zero) 3045 len = 12.0f / (float)Math.Sqrt(len);
2457 return Vector3.Zero; 3046 newAngVel *= len;
2458
2459 return _torque;
2460 } 3047 }
2461 3048
2462 set 3049 if (!m_isSelected)
2463 { 3050 {
2464 if (value.IsFinite()) 3051 if (Body != IntPtr.Zero)
2465 {
2466 m_taintTorque = value;
2467 _parent_scene.AddPhysicsActorTaint(this);
2468 }
2469 else
2470 { 3052 {
2471 m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name); 3053 if (m_disabled)
3054 enableBodySoft();
3055 else if (!d.BodyIsEnabled(Body))
3056 d.BodyEnable(Body);
3057
3058
3059 d.BodySetAngularVel(Body, newAngVel.X, newAngVel.Y, newAngVel.Z);
2472 } 3060 }
3061 //resetCollisionAccounting();
2473 } 3062 }
3063 m_rotationalVelocity = newAngVel;
2474 } 3064 }
2475 3065
2476 public override float CollisionScore 3066 private void changeVolumedetetion(bool newVolDtc)
2477 { 3067 {
2478 get { return m_collisionscore; } 3068 m_isVolumeDetect = newVolDtc;
2479 set { m_collisionscore = value; } 3069 m_fakeisVolumeDetect = newVolDtc;
3070 UpdateCollisionCatFlags();
3071 ApplyCollisionCatFlags();
2480 } 3072 }
2481 3073
2482 public override bool Kinematic 3074 protected void changeBuilding(bool newbuilding)
2483 { 3075 {
2484 get { return false; } 3076 // Check if we need to do anything
2485 set { } 3077 if (newbuilding == m_building)
2486 } 3078 return;
2487 3079
2488 public override Quaternion Orientation 3080 if ((bool)newbuilding)
2489 {
2490 get { return _orientation; }
2491 set
2492 { 3081 {
2493 if (QuaternionIsFinite(value)) 3082 m_building = true;
2494 _orientation = value; 3083 if (!childPrim)
2495 else 3084 DestroyBody();
2496 m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name); 3085 }
3086 else
3087 {
3088 m_building = false;
3089 CheckDelaySelect();
3090 if (!childPrim)
3091 MakeBody();
3092 }
3093 if (!childPrim && childrenPrim.Count > 0)
3094 {
3095 foreach (OdePrim prm in childrenPrim)
3096 prm.changeBuilding(m_building); // call directly
2497 } 3097 }
2498 } 3098 }
2499 3099
2500 private static bool QuaternionIsFinite(Quaternion q) 3100 public void changeSetVehicle(VehicleData vdata)
2501 {
2502 if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
2503 return false;
2504 if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
2505 return false;
2506 if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
2507 return false;
2508 if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
2509 return false;
2510 return true;
2511 }
2512
2513 public override Vector3 Acceleration
2514 { 3101 {
2515 get { return _acceleration; } 3102 if (m_vehicle == null)
2516 set { _acceleration = value; } 3103 m_vehicle = new ODEDynamics(this);
3104 m_vehicle.DoSetVehicle(vdata);
2517 } 3105 }
2518 3106
2519 public override void AddForce(Vector3 force, bool pushforce) 3107 private void changeVehicleType(int value)
2520 { 3108 {
2521 if (force.IsFinite()) 3109 if (value == (int)Vehicle.TYPE_NONE)
2522 { 3110 {
2523 lock (m_forcelist) 3111 if (m_vehicle != null)
2524 m_forcelist.Add(force); 3112 m_vehicle = null;
2525
2526 m_taintforce = true;
2527 } 3113 }
2528 else 3114 else
2529 { 3115 {
2530 m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name); 3116 if (m_vehicle == null)
3117 m_vehicle = new ODEDynamics(this);
3118
3119 m_vehicle.ProcessTypeChange((Vehicle)value);
2531 } 3120 }
2532 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
2533 } 3121 }
2534 3122
2535 public override void AddAngularForce(Vector3 force, bool pushforce) 3123 private void changeVehicleFloatParam(strVehicleFloatParam fp)
2536 { 3124 {
2537 if (force.IsFinite()) 3125 if (m_vehicle == null)
2538 { 3126 return;
2539 m_angularforcelist.Add(force); 3127
2540 m_taintaddangularforce = true; 3128 m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value);
2541 }
2542 else
2543 {
2544 m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name);
2545 }
2546 } 3129 }
2547 3130
2548 public override Vector3 RotationalVelocity 3131 private void changeVehicleVectorParam(strVehicleVectorParam vp)
2549 { 3132 {
2550 get 3133 if (m_vehicle == null)
2551 { 3134 return;
2552 Vector3 pv = Vector3.Zero; 3135 m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value);
2553 if (_zeroFlag) 3136 }
2554 return pv;
2555 m_lastUpdateSent = false;
2556
2557 if (m_rotationalVelocity.ApproxEquals(pv, 0.2f))
2558 return pv;
2559 3137
2560 return m_rotationalVelocity; 3138 private void changeVehicleRotationParam(strVehicleQuatParam qp)
2561 } 3139 {
2562 set 3140 if (m_vehicle == null)
2563 { 3141 return;
2564 if (value.IsFinite()) 3142 m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value);
2565 {
2566 m_rotationalVelocity = value;
2567 }
2568 else
2569 {
2570 m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name);
2571 }
2572 }
2573 } 3143 }
2574 3144
2575 public override void CrossingFailure() 3145 private void changeVehicleFlags(strVehicleBoolParam bp)
2576 { 3146 {
2577 m_crossingfailures++; 3147 if (m_vehicle == null)
2578 if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
2579 {
2580 base.RaiseOutOfBounds(_position);
2581 return; 3148 return;
2582 } 3149 m_vehicle.ProcessVehicleFlags(bp.param, bp.value);
2583 else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
2584 {
2585 m_log.Warn("[PHYSICS]: Too many crossing failures for: " + Name);
2586 }
2587 } 3150 }
2588 3151
2589 public override float Buoyancy 3152 private void changeBuoyancy(float b)
2590 { 3153 {
2591 get { return m_buoyancy; } 3154 m_buoyancy = b;
2592 set { m_buoyancy = value; }
2593 } 3155 }
2594 3156
2595 public override void link(PhysicsActor obj) 3157 private void changePIDTarget(Vector3 trg)
2596 { 3158 {
2597 m_taintparent = obj; 3159 m_PIDTarget = trg;
2598 } 3160 }
2599 3161
2600 public override void delink() 3162 private void changePIDTau(float tau)
2601 { 3163 {
2602 m_taintparent = null; 3164 m_PIDTau = tau;
2603 } 3165 }
2604 3166
2605 public override void LockAngularMotion(Vector3 axis) 3167 private void changePIDActive(bool val)
2606 { 3168 {
2607 // reverse the zero/non zero values for ODE. 3169 m_usePID = val;
2608 if (axis.IsFinite()) 3170 }
2609 { 3171
2610 axis.X = (axis.X > 0) ? 1f : 0f; 3172 private void changePIDHoverHeight(float val)
2611 axis.Y = (axis.Y > 0) ? 1f : 0f; 3173 {
2612 axis.Z = (axis.Z > 0) ? 1f : 0f; 3174 m_PIDHoverHeight = val;
2613 m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); 3175 if (val == 0)
2614 m_taintAngularLock = axis; 3176 m_useHoverPID = false;
2615 } 3177 }
2616 else 3178
2617 { 3179 private void changePIDHoverType(PIDHoverType type)
2618 m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name); 3180 {
2619 } 3181 m_PIDHoverType = type;
2620 } 3182 }
2621 3183
2622 internal void UpdatePositionAndVelocity() 3184 private void changePIDHoverTau(float tau)
3185 {
3186 m_PIDHoverTau = tau;
3187 }
3188
3189 private void changePIDHoverActive(bool active)
3190 {
3191 m_useHoverPID = active;
3192 }
3193
3194 #endregion
3195
3196 public void Move()
2623 { 3197 {
2624 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! 3198 if (!childPrim && m_isphysical && Body != IntPtr.Zero &&
2625 if (_parent == null) 3199 !m_disabled && !m_isSelected && !m_building && !m_outbounds)
2626 { 3200 {
2627 Vector3 pv = Vector3.Zero; 3201 if (!d.BodyIsEnabled(Body))
2628 bool lastZeroFlag = _zeroFlag; 3202 {
2629 float m_minvelocity = 0; 3203 // let vehicles sleep
2630 if (Body != (IntPtr)0) // FIXME -> or if it is a joint 3204 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
2631 { 3205 return;
2632 d.Vector3 vec = d.BodyGetPosition(Body); 3206
2633 d.Quaternion ori = d.BodyGetQuaternion(Body); 3207 if (++bodydisablecontrol < 20)
2634 d.Vector3 vel = d.BodyGetLinearVel(Body); 3208 return;
2635 d.Vector3 rotvel = d.BodyGetAngularVel(Body); 3209
2636 d.Vector3 torque = d.BodyGetTorque(Body); 3210
2637 _torque = new Vector3(torque.X, torque.Y, torque.Z); 3211 d.BodyEnable(Body);
2638 Vector3 l_position = Vector3.Zero; 3212 }
2639 Quaternion l_orientation = Quaternion.Identity; 3213
2640 3214 bodydisablecontrol = 0;
2641 // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!) 3215
2642 //if (vec.X < 0.0f) { vec.X = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } 3216 d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator
2643 //if (vec.Y < 0.0f) { vec.Y = 0.0f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } 3217
2644 //if (vec.X > 255.95f) { vec.X = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } 3218 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
2645 //if (vec.Y > 255.95f) { vec.Y = 255.95f; if (Body != (IntPtr)0) d.BodySetAngularVel(Body, 0, 0, 0); } 3219 {
2646 3220 // 'VEHICLES' are dealt with in ODEDynamics.cs
2647 m_lastposition = _position; 3221 m_vehicle.Step();
2648 m_lastorientation = _orientation; 3222 return;
2649 3223 }
2650 l_position.X = vec.X; 3224
2651 l_position.Y = vec.Y; 3225 float fx = 0;
2652 l_position.Z = vec.Z; 3226 float fy = 0;
2653 l_orientation.X = ori.X; 3227 float fz = 0;
2654 l_orientation.Y = ori.Y; 3228
2655 l_orientation.Z = ori.Z; 3229 float m_mass = _mass;
2656 l_orientation.W = ori.W; 3230
2657 3231 if (m_usePID && m_PIDTau > 0)
2658 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) 3232 {
3233 // for now position error
3234 _target_velocity =
3235 new Vector3(
3236 (m_PIDTarget.X - lpos.X),
3237 (m_PIDTarget.Y - lpos.Y),
3238 (m_PIDTarget.Z - lpos.Z)
3239 );
3240
3241 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.02f))
3242 {
3243 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3244 d.BodySetLinearVel(Body, 0, 0, 0);
3245 return;
3246 }
3247 else
2659 { 3248 {
2660 //base.RaiseOutOfBounds(l_position); 3249 _zeroFlag = false;
2661 3250
2662 if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds) 3251 float tmp = 1 / m_PIDTau;
3252 _target_velocity *= tmp;
3253
3254 // apply limits
3255 tmp = _target_velocity.Length();
3256 if (tmp > 50.0f)
2663 { 3257 {
2664 _position = l_position; 3258 tmp = 50 / tmp;
2665 //_parent_scene.remActivePrim(this); 3259 _target_velocity *= tmp;
2666 if (_parent == null)
2667 base.RequestPhysicsterseUpdate();
2668 return;
2669 } 3260 }
2670 else 3261 else if (tmp < 0.05f)
2671 { 3262 {
2672 if (_parent == null) 3263 tmp = 0.05f / tmp;
2673 base.RaiseOutOfBounds(l_position); 3264 _target_velocity *= tmp;
2674 return;
2675 } 3265 }
3266
3267 d.Vector3 vel = d.BodyGetLinearVel(Body);
3268 fx = (_target_velocity.X - vel.X) * m_invTimeStep;
3269 fy = (_target_velocity.Y - vel.Y) * m_invTimeStep;
3270 fz = (_target_velocity.Z - vel.Z) * m_invTimeStep;
3271// d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
2676 } 3272 }
3273 } // end if (m_usePID)
3274
3275 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
3276 else if (m_useHoverPID && m_PIDHoverTau != 0 && m_PIDHoverHeight != 0)
3277 {
2677 3278
2678 if (l_position.Z < 0) 3279 // Non-Vehicles have a limited set of Hover options.
3280 // determine what our target height really is based on HoverType
3281
3282 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(lpos.X, lpos.Y);
3283
3284 switch (m_PIDHoverType)
3285 {
3286 case PIDHoverType.Ground:
3287 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3288 break;
3289
3290 case PIDHoverType.GroundAndWater:
3291 m_waterHeight = _parent_scene.GetWaterLevel();
3292 if (m_groundHeight > m_waterHeight)
3293 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3294 else
3295 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
3296 break;
3297 } // end switch (m_PIDHoverType)
3298
3299 // don't go underground unless volumedetector
3300
3301 if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect)
2679 { 3302 {
2680 // This is so prim that get lost underground don't fall forever and suck up 3303 d.Vector3 vel = d.BodyGetLinearVel(Body);
2681 // 3304
2682 // Sim resources and memory. 3305 fz = (m_targetHoverHeight - lpos.Z);
2683 // Disables the prim's movement physics.... 3306
2684 // It's a hack and will generate a console message if it fails. 3307 // if error is zero, use position control; otherwise, velocity control
3308 if (Math.Abs(fz) < 0.01f)
3309 {
3310 d.BodySetPosition(Body, lpos.X, lpos.Y, m_targetHoverHeight);
3311 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
3312 }
3313 else
3314 {
3315 _zeroFlag = false;
3316 fz /= m_PIDHoverTau;
3317
3318 float tmp = Math.Abs(fz);
3319 if (tmp > 50)
3320 fz = 50 * Math.Sign(fz);
3321 else if (tmp < 0.1)
3322 fz = 0.1f * Math.Sign(fz);
3323
3324 fz = ((fz - vel.Z) * m_invTimeStep);
3325 }
3326 }
3327 }
3328 else
3329 {
3330 float b = (1.0f - m_buoyancy);
3331 fx = _parent_scene.gravityx * b;
3332 fy = _parent_scene.gravityy * b;
3333 fz = _parent_scene.gravityz * b;
3334 }
3335
3336 fx *= m_mass;
3337 fy *= m_mass;
3338 fz *= m_mass;
3339
3340 // constant force
3341 fx += m_force.X;
3342 fy += m_force.Y;
3343 fz += m_force.Z;
3344
3345 fx += m_forceacc.X;
3346 fy += m_forceacc.Y;
3347 fz += m_forceacc.Z;
3348
3349 m_forceacc = Vector3.Zero;
3350
3351 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
3352 if (fx != 0 || fy != 0 || fz != 0)
3353 {
3354 d.BodyAddForce(Body, fx, fy, fz);
3355 //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
3356 }
3357
3358 Vector3 trq;
3359
3360 trq = _torque;
3361 trq += m_angularForceacc;
3362 m_angularForceacc = Vector3.Zero;
3363 if (trq.X != 0 || trq.Y != 0 || trq.Z != 0)
3364 {
3365 d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z);
3366 }
3367 }
3368 else
3369 { // is not physical, or is not a body or is selected
3370 // _zeroPosition = d.BodyGetPosition(Body);
3371 return;
3372 //Console.WriteLine("Nothing " + Name);
3373
3374 }
3375 }
3376
3377 public void UpdatePositionAndVelocity()
3378 {
3379 if (_parent == null && !m_disabled && !m_building && !m_outbounds && Body != IntPtr.Zero)
3380 {
3381 if (d.BodyIsEnabled(Body) || !_zeroFlag)
3382 {
3383 bool lastZeroFlag = _zeroFlag;
2685 3384
2686 //IsPhysical = false; 3385 d.Vector3 lpos = d.GeomGetPosition(prim_geom);
2687 if (_parent == null)
2688 base.RaiseOutOfBounds(_position);
2689 3386
3387 // check outside region
3388 if (lpos.Z < -100 || lpos.Z > 100000f)
3389 {
3390 m_outbounds = true;
3391
3392 lpos.Z = Util.Clip(lpos.Z, -100f, 100000f);
2690 _acceleration.X = 0; 3393 _acceleration.X = 0;
2691 _acceleration.Y = 0; 3394 _acceleration.Y = 0;
2692 _acceleration.Z = 0; 3395 _acceleration.Z = 0;
@@ -2698,589 +3401,437 @@ Console.WriteLine(" JointCreateFixed");
2698 m_rotationalVelocity.Y = 0; 3401 m_rotationalVelocity.Y = 0;
2699 m_rotationalVelocity.Z = 0; 3402 m_rotationalVelocity.Z = 0;
2700 3403
2701 if (_parent == null) 3404 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
2702 base.RequestPhysicsterseUpdate(); 3405 d.BodySetAngularVel(Body, 0, 0, 0); // stop it
3406 d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere
3407 m_lastposition = _position;
3408 m_lastorientation = _orientation;
3409
3410 base.RequestPhysicsterseUpdate();
2703 3411
2704 m_throttleUpdates = false; 3412// throttleCounter = 0;
2705 throttleCounter = 0;
2706 _zeroFlag = true; 3413 _zeroFlag = true;
2707 //outofBounds = true; 3414
3415 disableBodySoft(); // disable it and colisions
3416 base.RaiseOutOfBounds(_position);
3417 return;
2708 } 3418 }
2709 3419
2710 //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)); 3420 if (lpos.X < 0f)
2711//Console.WriteLine("Adiff " + Name + " = " + Adiff);
2712 if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
2713 && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
2714 && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
2715// && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01))
2716 && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large
2717 { 3421 {
2718 _zeroFlag = true; 3422 _position.X = Util.Clip(lpos.X, -2f, -0.1f);
2719//Console.WriteLine("ZFT 2"); 3423 m_outbounds = true;
2720 m_throttleUpdates = false;
2721 } 3424 }
2722 else 3425 else if (lpos.X > _parent_scene.WorldExtents.X)
2723 { 3426 {
2724 //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString()); 3427 _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f);
2725 _zeroFlag = false; 3428 m_outbounds = true;
2726 m_lastUpdateSent = false;
2727 //m_throttleUpdates = false;
2728 } 3429 }
2729 3430 if (lpos.Y < 0f)
2730 if (_zeroFlag)
2731 { 3431 {
2732 _velocity.X = 0.0f; 3432 _position.Y = Util.Clip(lpos.Y, -2f, -0.1f);
2733 _velocity.Y = 0.0f; 3433 m_outbounds = true;
2734 _velocity.Z = 0.0f; 3434 }
2735 3435 else if (lpos.Y > _parent_scene.WorldExtents.Y)
2736 _acceleration.X = 0; 3436 {
2737 _acceleration.Y = 0; 3437 _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f);
2738 _acceleration.Z = 0; 3438 m_outbounds = true;
2739 3439 }
2740 //_orientation.w = 0f;
2741 //_orientation.X = 0f;
2742 //_orientation.Y = 0f;
2743 //_orientation.Z = 0f;
2744 m_rotationalVelocity.X = 0;
2745 m_rotationalVelocity.Y = 0;
2746 m_rotationalVelocity.Z = 0;
2747 if (!m_lastUpdateSent)
2748 {
2749 m_throttleUpdates = false;
2750 throttleCounter = 0;
2751 m_rotationalVelocity = pv;
2752 3440
2753 if (_parent == null) 3441 if (m_outbounds)
2754 { 3442 {
2755 base.RequestPhysicsterseUpdate(); 3443 m_lastposition = _position;
2756 } 3444 m_lastorientation = _orientation;
3445
3446 d.Vector3 dtmp = d.BodyGetAngularVel(Body);
3447 m_rotationalVelocity.X = dtmp.X;
3448 m_rotationalVelocity.Y = dtmp.Y;
3449 m_rotationalVelocity.Z = dtmp.Z;
3450
3451 dtmp = d.BodyGetLinearVel(Body);
3452 _velocity.X = dtmp.X;
3453 _velocity.Y = dtmp.Y;
3454 _velocity.Z = dtmp.Z;
3455
3456 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
3457 d.BodySetAngularVel(Body, 0, 0, 0);
3458 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
3459 disableBodySoft(); // stop collisions
3460 UnSubscribeEvents();
3461
3462 base.RequestPhysicsterseUpdate();
3463 return;
3464 }
2757 3465
2758 m_lastUpdateSent = true; 3466 d.Quaternion ori;
2759 } 3467 d.GeomCopyQuaternion(prim_geom, out ori);
3468
3469 // decide if moving
3470 // use positions since this are integrated quantities
3471 // tolerance values depende a lot on simulation noise...
3472 // use simple math.abs since we dont need to be exact
3473
3474 if (
3475 (Math.Abs(_position.X - lpos.X) < 0.001f)
3476 && (Math.Abs(_position.Y - lpos.Y) < 0.001f)
3477 && (Math.Abs(_position.Z - lpos.Z) < 0.001f)
3478 && (Math.Abs(_orientation.X - ori.X) < 0.0001f)
3479 && (Math.Abs(_orientation.Y - ori.Y) < 0.0001f)
3480 && (Math.Abs(_orientation.Z - ori.Z) < 0.0001f) // ignore W
3481 )
3482 {
3483 _zeroFlag = true;
2760 } 3484 }
2761 else 3485 else
2762 { 3486 _zeroFlag = false;
2763 if (lastZeroFlag != _zeroFlag)
2764 {
2765 if (_parent == null)
2766 {
2767 base.RequestPhysicsterseUpdate();
2768 }
2769 }
2770
2771 m_lastVelocity = _velocity;
2772 3487
2773 _position = l_position; 3488 // update velocities and aceleration
3489 if (!(_zeroFlag && lastZeroFlag))
3490 {
3491 d.Vector3 vel = d.BodyGetLinearVel(Body);
2774 3492
2775 _velocity.X = vel.X; 3493 _acceleration = _velocity;
2776 _velocity.Y = vel.Y;
2777 _velocity.Z = vel.Z;
2778 3494
2779 _acceleration = ((_velocity - m_lastVelocity) / 0.1f); 3495 if ((Math.Abs(vel.X) < 0.001f) &&
2780 _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f); 3496 (Math.Abs(vel.Y) < 0.001f) &&
2781 //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString()); 3497 (Math.Abs(vel.Z) < 0.001f))
2782
2783 // Note here that linearvelocity is affecting angular velocity... so I'm guessing this is a vehicle specific thing...
2784 // it does make sense to do this for tiny little instabilities with physical prim, however 0.5m/frame is fairly large.
2785 // 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
2786 // adding these logical exclusion situations to maintain this where I think it was intended to be.
2787 if (m_throttleUpdates || m_usePID || (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) || (Amotor != IntPtr.Zero))
2788 { 3498 {
2789 m_minvelocity = 0.5f; 3499 _velocity = Vector3.Zero;
3500 float t = -m_invTimeStep;
3501 _acceleration = _acceleration * t;
2790 } 3502 }
2791 else 3503 else
2792 { 3504 {
2793 m_minvelocity = 0.02f; 3505 _velocity.X = vel.X;
3506 _velocity.Y = vel.Y;
3507 _velocity.Z = vel.Z;
3508 _acceleration = (_velocity - _acceleration) * m_invTimeStep;
3509 }
3510
3511 if ((Math.Abs(_acceleration.X) < 0.01f) &&
3512 (Math.Abs(_acceleration.Y) < 0.01f) &&
3513 (Math.Abs(_acceleration.Z) < 0.01f))
3514 {
3515 _acceleration = Vector3.Zero;
2794 } 3516 }
2795 3517
2796 if (_velocity.ApproxEquals(pv, m_minvelocity)) 3518 if ((Math.Abs(_orientation.X - ori.X) < 0.0001) &&
3519 (Math.Abs(_orientation.Y - ori.Y) < 0.0001) &&
3520 (Math.Abs(_orientation.Z - ori.Z) < 0.0001)
3521 )
2797 { 3522 {
2798 m_rotationalVelocity = pv; 3523 m_rotationalVelocity = Vector3.Zero;
2799 } 3524 }
2800 else 3525 else
2801 { 3526 {
2802 m_rotationalVelocity = new Vector3(rotvel.X, rotvel.Y, rotvel.Z); 3527 vel = d.BodyGetAngularVel(Body);
3528 m_rotationalVelocity.X = vel.X;
3529 m_rotationalVelocity.Y = vel.Y;
3530 m_rotationalVelocity.Z = vel.Z;
2803 } 3531 }
3532 }
2804 3533
2805 //m_log.Debug("ODE: " + m_rotationalVelocity.ToString()); 3534 if (_zeroFlag)
2806 _orientation.X = ori.X; 3535 {
2807 _orientation.Y = ori.Y; 3536 if (lastZeroFlag)
2808 _orientation.Z = ori.Z;
2809 _orientation.W = ori.W;
2810 m_lastUpdateSent = false;
2811 if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
2812 { 3537 {
2813 if (_parent == null) 3538 _velocity = Vector3.Zero;
2814 { 3539 _acceleration = Vector3.Zero;
2815 base.RequestPhysicsterseUpdate(); 3540 m_rotationalVelocity = Vector3.Zero;
2816 }
2817 } 3541 }
2818 else 3542
3543 if (!m_lastUpdateSent)
2819 { 3544 {
2820 throttleCounter++; 3545 base.RequestPhysicsterseUpdate();
3546 if (lastZeroFlag)
3547 m_lastUpdateSent = true;
2821 } 3548 }
3549 return;
2822 } 3550 }
2823 m_lastposition = l_position;
2824 }
2825 else
2826 {
2827 // Not a body.. so Make sure the client isn't interpolating
2828 _velocity.X = 0;
2829 _velocity.Y = 0;
2830 _velocity.Z = 0;
2831 3551
2832 _acceleration.X = 0; 3552 _position.X = lpos.X;
2833 _acceleration.Y = 0; 3553 _position.Y = lpos.Y;
2834 _acceleration.Z = 0; 3554 _position.Z = lpos.Z;
2835 3555
2836 m_rotationalVelocity.X = 0; 3556 _orientation.X = ori.X;
2837 m_rotationalVelocity.Y = 0; 3557 _orientation.Y = ori.Y;
2838 m_rotationalVelocity.Z = 0; 3558 _orientation.Z = ori.Z;
2839 _zeroFlag = true; 3559 _orientation.W = ori.W;
3560 base.RequestPhysicsterseUpdate();
3561 m_lastUpdateSent = false;
2840 } 3562 }
2841 } 3563 }
2842 } 3564 }
2843 3565
2844 public override bool FloatOnWater 3566 internal static bool QuaternionIsFinite(Quaternion q)
2845 { 3567 {
2846 set { 3568 if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
2847 m_taintCollidesWater = value; 3569 return false;
2848 _parent_scene.AddPhysicsActorTaint(this); 3570 if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
2849 } 3571 return false;
3572 if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
3573 return false;
3574 if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
3575 return false;
3576 return true;
2850 } 3577 }
2851 3578
2852 public override void SetMomentum(Vector3 momentum) 3579 internal static void DMassSubPartFromObj(ref d.Mass part, ref d.Mass theobj)
2853 { 3580 {
2854 } 3581 // assumes object center of mass is zero
2855 3582 float smass = part.mass;
2856 public override Vector3 PIDTarget 3583 theobj.mass -= smass;
2857 {
2858 set
2859 {
2860 if (value.IsFinite())
2861 {
2862 m_PIDTarget = value;
2863 }
2864 else
2865 m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name);
2866 }
2867 }
2868 public override bool PIDActive { set { m_usePID = value; } }
2869 public override float PIDTau { set { m_PIDTau = value; } }
2870
2871 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
2872 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
2873 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
2874 public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
2875
2876 public override Quaternion APIDTarget{ set { return; } }
2877 3584
2878 public override bool APIDActive{ set { return; } } 3585 smass *= 1.0f / (theobj.mass); ;
2879 3586
2880 public override float APIDStrength{ set { return; } } 3587 theobj.c.X -= part.c.X * smass;
3588 theobj.c.Y -= part.c.Y * smass;
3589 theobj.c.Z -= part.c.Z * smass;
2881 3590
2882 public override float APIDDamping{ set { return; } } 3591 theobj.I.M00 -= part.I.M00;
3592 theobj.I.M01 -= part.I.M01;
3593 theobj.I.M02 -= part.I.M02;
3594 theobj.I.M10 -= part.I.M10;
3595 theobj.I.M11 -= part.I.M11;
3596 theobj.I.M12 -= part.I.M12;
3597 theobj.I.M20 -= part.I.M20;
3598 theobj.I.M21 -= part.I.M21;
3599 theobj.I.M22 -= part.I.M22;
3600 }
2883 3601
2884 private void createAMotor(Vector3 axis) 3602 private void donullchange()
2885 { 3603 {
2886 if (Body == IntPtr.Zero) 3604 }
2887 return;
2888 3605
2889 if (Amotor != IntPtr.Zero) 3606 public bool DoAChange(changes what, object arg)
3607 {
3608 if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.AddPhysRep && what != changes.Remove)
2890 { 3609 {
2891 d.JointDestroy(Amotor); 3610 return false;
2892 Amotor = IntPtr.Zero;
2893 } 3611 }
2894 3612
2895 float axisnum = 3; 3613 // nasty switch
3614 switch (what)
3615 {
3616 case changes.Add:
3617 changeadd();
3618 break;
2896 3619
2897 axisnum = (axisnum - (axis.X + axis.Y + axis.Z)); 3620 case changes.AddPhysRep:
3621 changeAddPhysRep((ODEPhysRepData)arg);
3622 break;
2898 3623
2899 // PhysicsVector totalSize = new PhysicsVector(_size.X, _size.Y, _size.Z); 3624 case changes.Remove:
3625 //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff...
3626 //When we return true, it destroys all of the prims in the linkset anyway
3627 if (_parent != null)
3628 {
3629 OdePrim parent = (OdePrim)_parent;
3630 parent.ChildRemove(this, false);
3631 }
3632 else
3633 ChildRemove(this, false);
2900 3634
2901 3635 m_vehicle = null;
2902 // Inverse Inertia Matrix, set the X, Y, and/r Z inertia to 0 then invert it again. 3636 RemoveGeom();
2903 d.Mass objMass; 3637 m_targetSpace = IntPtr.Zero;
2904 d.MassSetZero(out objMass); 3638 UnSubscribeEvents();
2905 DMassCopy(ref pMass, ref objMass); 3639 return true;
2906 3640
2907 //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); 3641 case changes.Link:
3642 OdePrim tmp = (OdePrim)arg;
3643 changeLink(tmp);
3644 break;
2908 3645
2909 Matrix4 dMassMat = FromDMass(objMass); 3646 case changes.DeLink:
3647 changeLink(null);
3648 break;
2910 3649
2911 Matrix4 mathmat = Inverse(dMassMat); 3650 case changes.Position:
3651 changePosition((Vector3)arg);
3652 break;
2912 3653
2913 /* 3654 case changes.Orientation:
2914 //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]); 3655 changeOrientation((Quaternion)arg);
3656 break;
2915 3657
2916 mathmat = Inverse(mathmat); 3658 case changes.PosOffset:
3659 donullchange();
3660 break;
2917 3661
3662 case changes.OriOffset:
3663 donullchange();
3664 break;
2918 3665
2919 objMass = FromMatrix4(mathmat, ref objMass); 3666 case changes.Velocity:
2920 //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); 3667 changevelocity((Vector3)arg);
3668 break;
2921 3669
2922 mathmat = Inverse(mathmat); 3670// case changes.Acceleration:
2923 */ 3671// changeacceleration((Vector3)arg);
2924 if (axis.X == 0) 3672// break;
2925 {
2926 mathmat.M33 = 50.0000001f;
2927 //objMass.I.M22 = 0;
2928 }
2929 if (axis.Y == 0)
2930 {
2931 mathmat.M22 = 50.0000001f;
2932 //objMass.I.M11 = 0;
2933 }
2934 if (axis.Z == 0)
2935 {
2936 mathmat.M11 = 50.0000001f;
2937 //objMass.I.M00 = 0;
2938 }
2939
2940
2941 3673
2942 mathmat = Inverse(mathmat); 3674 case changes.AngVelocity:
2943 objMass = FromMatrix4(mathmat, ref objMass); 3675 changeangvelocity((Vector3)arg);
2944 //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); 3676 break;
2945
2946 //return;
2947 if (d.MassCheck(ref objMass))
2948 {
2949 d.BodySetMass(Body, ref objMass);
2950 }
2951 else
2952 {
2953 //m_log.Debug("[PHYSICS]: Mass invalid, ignoring");
2954 }
2955 3677
2956 if (axisnum <= 0) 3678 case changes.Force:
2957 return; 3679 changeForce((Vector3)arg);
2958 // int dAMotorEuler = 1; 3680 break;
2959 3681
2960 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); 3682 case changes.Torque:
2961 d.JointAttach(Amotor, Body, IntPtr.Zero); 3683 changeSetTorque((Vector3)arg);
2962 d.JointSetAMotorMode(Amotor, 0); 3684 break;
2963 3685
2964 d.JointSetAMotorNumAxes(Amotor,(int)axisnum); 3686 case changes.AddForce:
2965 int i = 0; 3687 changeAddForce((Vector3)arg);
3688 break;
2966 3689
2967 if (axis.X == 0) 3690 case changes.AddAngForce:
2968 { 3691 changeAddAngularImpulse((Vector3)arg);
2969 d.JointSetAMotorAxis(Amotor, i, 0, 1, 0, 0); 3692 break;
2970 i++;
2971 }
2972 3693
2973 if (axis.Y == 0) 3694 case changes.AngLock:
2974 { 3695 changeAngularLock((Vector3)arg);
2975 d.JointSetAMotorAxis(Amotor, i, 0, 0, 1, 0); 3696 break;
2976 i++;
2977 }
2978 3697
2979 if (axis.Z == 0) 3698 case changes.Size:
2980 { 3699 changeSize((Vector3)arg);
2981 d.JointSetAMotorAxis(Amotor, i, 0, 0, 0, 1); 3700 break;
2982 i++;
2983 }
2984 3701
2985 for (int j = 0; j < (int)axisnum; j++) 3702 case changes.Shape:
2986 { 3703 changeShape((PrimitiveBaseShape)arg);
2987 //d.JointSetAMotorAngle(Amotor, j, 0); 3704 break;
2988 }
2989 3705
2990 //d.JointSetAMotorAngle(Amotor, 1, 0); 3706 case changes.PhysRepData:
2991 //d.JointSetAMotorAngle(Amotor, 2, 0); 3707 changePhysRepData((ODEPhysRepData) arg);
3708 break;
2992 3709
2993 // These lowstops and high stops are effectively (no wiggle room) 3710 case changes.CollidesWater:
2994 d.JointSetAMotorParam(Amotor, (int)dParam.LowStop, -0f); 3711 changeFloatOnWater((bool)arg);
2995 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop3, -0f); 3712 break;
2996 d.JointSetAMotorParam(Amotor, (int)dParam.LoStop2, -0f);
2997 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop, 0f);
2998 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop3, 0f);
2999 d.JointSetAMotorParam(Amotor, (int)dParam.HiStop2, 0f);
3000 //d.JointSetAMotorParam(Amotor, (int) dParam.Vel, 9000f);
3001 d.JointSetAMotorParam(Amotor, (int)dParam.FudgeFactor, 0f);
3002 d.JointSetAMotorParam(Amotor, (int)dParam.FMax, Mass * 50f);//
3003 }
3004 3713
3005 private Matrix4 FromDMass(d.Mass pMass) 3714 case changes.VolumeDtc:
3006 { 3715 changeVolumedetetion((bool)arg);
3007 Matrix4 obj; 3716 break;
3008 obj.M11 = pMass.I.M00;
3009 obj.M12 = pMass.I.M01;
3010 obj.M13 = pMass.I.M02;
3011 obj.M14 = 0;
3012 obj.M21 = pMass.I.M10;
3013 obj.M22 = pMass.I.M11;
3014 obj.M23 = pMass.I.M12;
3015 obj.M24 = 0;
3016 obj.M31 = pMass.I.M20;
3017 obj.M32 = pMass.I.M21;
3018 obj.M33 = pMass.I.M22;
3019 obj.M34 = 0;
3020 obj.M41 = 0;
3021 obj.M42 = 0;
3022 obj.M43 = 0;
3023 obj.M44 = 1;
3024 return obj;
3025 }
3026 3717
3027 private d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj) 3718 case changes.Phantom:
3028 { 3719 changePhantomStatus((bool)arg);
3029 obj.I.M00 = pMat[0, 0]; 3720 break;
3030 obj.I.M01 = pMat[0, 1];
3031 obj.I.M02 = pMat[0, 2];
3032 obj.I.M10 = pMat[1, 0];
3033 obj.I.M11 = pMat[1, 1];
3034 obj.I.M12 = pMat[1, 2];
3035 obj.I.M20 = pMat[2, 0];
3036 obj.I.M21 = pMat[2, 1];
3037 obj.I.M22 = pMat[2, 2];
3038 return obj;
3039 }
3040 3721
3041 public override void SubscribeEvents(int ms) 3722 case changes.Physical:
3042 { 3723 changePhysicsStatus((bool)arg);
3043 m_eventsubscription = ms; 3724 break;
3044 _parent_scene.AddCollisionEventReporting(this);
3045 }
3046 3725
3047 public override void UnSubscribeEvents() 3726 case changes.Selected:
3048 { 3727 changeSelectedStatus((bool)arg);
3049 _parent_scene.RemoveCollisionEventReporting(this); 3728 break;
3050 m_eventsubscription = 0;
3051 }
3052 3729
3053 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) 3730 case changes.disabled:
3054 { 3731 changeDisable((bool)arg);
3055 CollisionEventsThisFrame.AddCollider(CollidedWith, contact); 3732 break;
3056 }
3057 3733
3058 public void SendCollisions() 3734 case changes.building:
3059 { 3735 changeBuilding((bool)arg);
3060 if (m_collisionsOnPreviousFrame || CollisionEventsThisFrame.Count > 0) 3736 break;
3061 {
3062 base.SendCollisionUpdate(CollisionEventsThisFrame);
3063 3737
3064 if (CollisionEventsThisFrame.Count > 0) 3738 case changes.VehicleType:
3065 { 3739 changeVehicleType((int)arg);
3066 m_collisionsOnPreviousFrame = true; 3740 break;
3067 CollisionEventsThisFrame.Clear();
3068 }
3069 else
3070 {
3071 m_collisionsOnPreviousFrame = false;
3072 }
3073 }
3074 }
3075 3741
3076 public override bool SubscribedEvents() 3742 case changes.VehicleFlags:
3077 { 3743 changeVehicleFlags((strVehicleBoolParam) arg);
3078 if (m_eventsubscription > 0) 3744 break;
3079 return true;
3080 return false;
3081 }
3082 3745
3083 public static Matrix4 Inverse(Matrix4 pMat) 3746 case changes.VehicleFloatParam:
3084 { 3747 changeVehicleFloatParam((strVehicleFloatParam) arg);
3085 if (determinant3x3(pMat) == 0) 3748 break;
3086 {
3087 return Matrix4.Identity; // should probably throw an error. singluar matrix inverse not possible
3088 }
3089 3749
3090 return (Adjoint(pMat) / determinant3x3(pMat)); 3750 case changes.VehicleVectorParam:
3091 } 3751 changeVehicleVectorParam((strVehicleVectorParam) arg);
3752 break;
3092 3753
3093 public static Matrix4 Adjoint(Matrix4 pMat) 3754 case changes.VehicleRotationParam:
3094 { 3755 changeVehicleRotationParam((strVehicleQuatParam) arg);
3095 Matrix4 adjointMatrix = new Matrix4(); 3756 break;
3096 for (int i=0; i<4; i++)
3097 {
3098 for (int j=0; j<4; j++)
3099 {
3100 Matrix4SetValue(ref adjointMatrix, i, j, (float)(Math.Pow(-1, i + j) * (determinant3x3(Minor(pMat, i, j)))));
3101 }
3102 }
3103 3757
3104 adjointMatrix = Transpose(adjointMatrix); 3758 case changes.SetVehicle:
3105 return adjointMatrix; 3759 changeSetVehicle((VehicleData) arg);
3106 } 3760 break;
3107 3761
3108 public static Matrix4 Minor(Matrix4 matrix, int iRow, int iCol) 3762 case changes.Buoyancy:
3109 { 3763 changeBuoyancy((float)arg);
3110 Matrix4 minor = new Matrix4(); 3764 break;
3111 int m = 0, n = 0;
3112 for (int i = 0; i < 4; i++)
3113 {
3114 if (i == iRow)
3115 continue;
3116 n = 0;
3117 for (int j = 0; j < 4; j++)
3118 {
3119 if (j == iCol)
3120 continue;
3121 Matrix4SetValue(ref minor, m,n, matrix[i, j]);
3122 n++;
3123 }
3124 m++;
3125 }
3126 3765
3127 return minor; 3766 case changes.PIDTarget:
3128 } 3767 changePIDTarget((Vector3)arg);
3768 break;
3129 3769
3130 public static Matrix4 Transpose(Matrix4 pMat) 3770 case changes.PIDTau:
3131 { 3771 changePIDTau((float)arg);
3132 Matrix4 transposeMatrix = new Matrix4(); 3772 break;
3133 for (int i = 0; i < 4; i++)
3134 for (int j = 0; j < 4; j++)
3135 Matrix4SetValue(ref transposeMatrix, i, j, pMat[j, i]);
3136 return transposeMatrix;
3137 }
3138 3773
3139 public static void Matrix4SetValue(ref Matrix4 pMat, int r, int c, float val) 3774 case changes.PIDActive:
3140 { 3775 changePIDActive((bool)arg);
3141 switch (r) 3776 break;
3142 {
3143 case 0:
3144 switch (c)
3145 {
3146 case 0:
3147 pMat.M11 = val;
3148 break;
3149 case 1:
3150 pMat.M12 = val;
3151 break;
3152 case 2:
3153 pMat.M13 = val;
3154 break;
3155 case 3:
3156 pMat.M14 = val;
3157 break;
3158 }
3159 3777
3778 case changes.PIDHoverHeight:
3779 changePIDHoverHeight((float)arg);
3160 break; 3780 break;
3161 case 1:
3162 switch (c)
3163 {
3164 case 0:
3165 pMat.M21 = val;
3166 break;
3167 case 1:
3168 pMat.M22 = val;
3169 break;
3170 case 2:
3171 pMat.M23 = val;
3172 break;
3173 case 3:
3174 pMat.M24 = val;
3175 break;
3176 }
3177 3781
3782 case changes.PIDHoverType:
3783 changePIDHoverType((PIDHoverType)arg);
3178 break; 3784 break;
3179 case 2:
3180 switch (c)
3181 {
3182 case 0:
3183 pMat.M31 = val;
3184 break;
3185 case 1:
3186 pMat.M32 = val;
3187 break;
3188 case 2:
3189 pMat.M33 = val;
3190 break;
3191 case 3:
3192 pMat.M34 = val;
3193 break;
3194 }
3195 3785
3786 case changes.PIDHoverTau:
3787 changePIDHoverTau((float)arg);
3196 break; 3788 break;
3197 case 3:
3198 switch (c)
3199 {
3200 case 0:
3201 pMat.M41 = val;
3202 break;
3203 case 1:
3204 pMat.M42 = val;
3205 break;
3206 case 2:
3207 pMat.M43 = val;
3208 break;
3209 case 3:
3210 pMat.M44 = val;
3211 break;
3212 }
3213 3789
3790 case changes.PIDHoverActive:
3791 changePIDHoverActive((bool)arg);
3792 break;
3793
3794 case changes.Null:
3795 donullchange();
3796 break;
3797
3798
3799
3800 default:
3801 donullchange();
3214 break; 3802 break;
3215 } 3803 }
3804 return false;
3216 } 3805 }
3217 3806
3218 private static float determinant3x3(Matrix4 pMat) 3807 public void AddChange(changes what, object arg)
3219 { 3808 {
3220 float det = 0; 3809 _parent_scene.AddChange((PhysicsActor) this, what, arg);
3221 float diag1 = pMat[0, 0]*pMat[1, 1]*pMat[2, 2];
3222 float diag2 = pMat[0, 1]*pMat[2, 1]*pMat[2, 0];
3223 float diag3 = pMat[0, 2]*pMat[1, 0]*pMat[2, 1];
3224 float diag4 = pMat[2, 0]*pMat[1, 1]*pMat[0, 2];
3225 float diag5 = pMat[2, 1]*pMat[1, 2]*pMat[0, 0];
3226 float diag6 = pMat[2, 2]*pMat[1, 0]*pMat[0, 1];
3227
3228 det = diag1 + diag2 + diag3 - (diag4 + diag5 + diag6);
3229 return det;
3230 }
3231
3232 private static void DMassCopy(ref d.Mass src, ref d.Mass dst)
3233 {
3234 dst.c.W = src.c.W;
3235 dst.c.X = src.c.X;
3236 dst.c.Y = src.c.Y;
3237 dst.c.Z = src.c.Z;
3238 dst.mass = src.mass;
3239 dst.I.M00 = src.I.M00;
3240 dst.I.M01 = src.I.M01;
3241 dst.I.M02 = src.I.M02;
3242 dst.I.M10 = src.I.M10;
3243 dst.I.M11 = src.I.M11;
3244 dst.I.M12 = src.I.M12;
3245 dst.I.M20 = src.I.M20;
3246 dst.I.M21 = src.I.M21;
3247 dst.I.M22 = src.I.M22;
3248 } 3810 }
3249 3811
3250 public override void SetMaterial(int pMaterial) 3812
3813 private struct strVehicleBoolParam
3251 { 3814 {
3252 m_material = pMaterial; 3815 public int param;
3816 public bool value;
3253 } 3817 }
3254 3818
3255 private void CheckMeshAsset() 3819 private struct strVehicleFloatParam
3256 { 3820 {
3257 if (_pbs.SculptEntry && !m_assetFailed && _pbs.SculptTexture != UUID.Zero) 3821 public int param;
3258 { 3822 public float value;
3259 m_assetFailed = true;
3260 Util.FireAndForget(delegate
3261 {
3262 RequestAssetDelegate assetProvider = _parent_scene.RequestAssetMethod;
3263 if (assetProvider != null)
3264 assetProvider(_pbs.SculptTexture, MeshAssetReveived);
3265 });
3266 }
3267 } 3823 }
3268 3824
3269 void MeshAssetReveived(AssetBase asset) 3825 private struct strVehicleQuatParam
3270 { 3826 {
3271 if (asset.Data != null && asset.Data.Length > 0) 3827 public int param;
3272 { 3828 public Quaternion value;
3273 if (!_pbs.SculptEntry) 3829 }
3274 return;
3275 if (_pbs.SculptTexture.ToString() != asset.ID)
3276 return;
3277 3830
3278 _pbs.SculptData = new byte[asset.Data.Length]; 3831 private struct strVehicleVectorParam
3279 asset.Data.CopyTo(_pbs.SculptData, 0); 3832 {
3280 m_assetFailed = false; 3833 public int param;
3281 m_taintshape = true; 3834 public Vector3 value;
3282 _parent_scene.AddPhysicsActorTaint(this); 3835 }
3283 }
3284 }
3285 } 3836 }
3286} \ No newline at end of file 3837}
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
index 7a50c4c..03048a4 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
@@ -25,26 +25,21 @@
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */ 26 */
27 27
28//#define USE_DRAWSTUFF
29//#define SPAM 28//#define SPAM
30 29
31using System; 30using System;
32using System.Collections.Generic; 31using System.Collections.Generic;
33using System.Diagnostics;
34using System.IO;
35using System.Linq;
36using System.Reflection; 32using System.Reflection;
37using System.Runtime.InteropServices; 33using System.Runtime.InteropServices;
38using System.Threading; 34using System.Threading;
35using System.IO;
36using System.Diagnostics;
39using log4net; 37using log4net;
40using Nini.Config; 38using Nini.Config;
41using Ode.NET; 39using OdeAPI;
42using OpenMetaverse;
43#if USE_DRAWSTUFF
44using Drawstuff.NET;
45#endif
46using OpenSim.Framework; 40using OpenSim.Framework;
47using OpenSim.Region.Physics.Manager; 41using OpenSim.Region.Physics.Manager;
42using OpenMetaverse;
48 43
49namespace OpenSim.Region.Physics.OdePlugin 44namespace OpenSim.Region.Physics.OdePlugin
50{ 45{
@@ -55,29 +50,42 @@ namespace OpenSim.Region.Physics.OdePlugin
55 End = 2 50 End = 2
56 } 51 }
57 52
58// public struct sCollisionData 53 public struct sCollisionData
59// { 54 {
60// public uint ColliderLocalId; 55 public uint ColliderLocalId;
61// public uint CollidedWithLocalId; 56 public uint CollidedWithLocalId;
62// public int NumberOfCollisions; 57 public int NumberOfCollisions;
63// public int CollisionType; 58 public int CollisionType;
64// public int StatusIndicator; 59 public int StatusIndicator;
65// public int lastframe; 60 public int lastframe;
66// } 61 }
62
63
64 // colision flags of things others can colide with
65 // rays, sensors, probes removed since can't be colided with
66 // The top space where things are placed provided further selection
67 // ie physical are in active space nonphysical in static
68 // this should be exclusive as possible
67 69
68 [Flags] 70 [Flags]
69 public enum CollisionCategories : int 71 public enum CollisionCategories : uint
70 { 72 {
71 Disabled = 0, 73 Disabled = 0,
72 Geom = 0x00000001, 74 //by 'things' types
73 Body = 0x00000002, 75 Space = 0x01,
74 Space = 0x00000004, 76 Geom = 0x02, // aka prim/part
75 Character = 0x00000008, 77 Character = 0x04,
76 Land = 0x00000010, 78 Land = 0x08,
77 Water = 0x00000020, 79 Water = 0x010,
78 Wind = 0x00000040, 80
79 Sensor = 0x00000080, 81 // by state
80 Selected = 0x00000100 82 Phantom = 0x01000,
83 VolumeDtc = 0x02000,
84 Selected = 0x04000,
85 NoShape = 0x08000,
86
87
88 All = 0xffffffff
81 } 89 }
82 90
83 /// <summary> 91 /// <summary>
@@ -98,400 +106,213 @@ namespace OpenSim.Region.Physics.OdePlugin
98 /// <summary></summary> 106 /// <summary></summary>
99 Plastic = 5, 107 Plastic = 5,
100 /// <summary></summary> 108 /// <summary></summary>
101 Rubber = 6 109 Rubber = 6,
110
111 light = 7 // compatibility with old viewers
112 }
113
114 public enum changes : int
115 {
116 Add = 0, // arg null. finishs the prim creation. should be used internally only ( to remove later ?)
117 Remove,
118 Link, // arg AuroraODEPrim new parent prim or null to delink. Makes the prim part of a object with prim parent as root
119 // or removes from a object if arg is null
120 DeLink,
121 Position, // arg Vector3 new position in world coords. Changes prim position. Prim must know if it is root or child
122 Orientation, // arg Quaternion new orientation in world coords. Changes prim position. Prim must know it it is root or child
123 PosOffset, // not in use
124 // arg Vector3 new position in local coords. Changes prim position in object
125 OriOffset, // not in use
126 // arg Vector3 new position in local coords. Changes prim position in object
127 Velocity,
128 AngVelocity,
129 Acceleration,
130 Force,
131 Torque,
132 Momentum,
133
134 AddForce,
135 AddAngForce,
136 AngLock,
137
138 Buoyancy,
139
140 PIDTarget,
141 PIDTau,
142 PIDActive,
143
144 PIDHoverHeight,
145 PIDHoverType,
146 PIDHoverTau,
147 PIDHoverActive,
148
149 Size,
150 Shape,
151 PhysRepData,
152 AddPhysRep,
153
154 CollidesWater,
155 VolumeDtc,
156
157 Physical,
158 Phantom,
159 Selected,
160 disabled,
161 building,
162
163 VehicleType,
164 VehicleFloatParam,
165 VehicleVectorParam,
166 VehicleRotationParam,
167 VehicleFlags,
168 SetVehicle,
169
170 Null //keep this last used do dim the methods array. does nothing but pulsing the prim
102 } 171 }
103 172
173 public struct ODEchangeitem
174 {
175 public PhysicsActor actor;
176 public OdeCharacter character;
177 public changes what;
178 public Object arg;
179 }
180
104 public class OdeScene : PhysicsScene 181 public class OdeScene : PhysicsScene
105 { 182 {
106 private readonly ILog m_log; 183 private readonly ILog m_log;
107 // private Dictionary<string, sCollisionData> m_storedCollisions = new Dictionary<string, sCollisionData>(); 184 // private Dictionary<string, sCollisionData> m_storedCollisions = new Dictionary<string, sCollisionData>();
108 185
109 /// <summary> 186 public bool OdeUbitLib = false;
110 /// Provide a sync object so that only one thread calls d.Collide() at a time across all OdeScene instances. 187// private int threadid = 0;
111 /// </summary> 188 private Random fluidRandomizer = new Random(Environment.TickCount);
112 /// <remarks>
113 /// With ODE as of r1755 (though also tested on r1860), only one thread can call d.Collide() at a
114 /// time, even where physics objects are in entirely different ODE worlds. This is because generating contacts
115 /// uses a static cache at the ODE level.
116 ///
117 /// Without locking, simulators running multiple regions will eventually crash with a native stack trace similar
118 /// to
119 ///
120 /// mono() [0x489171]
121 /// mono() [0x4d154f]
122 /// /lib/x86_64-linux-gnu/libpthread.so.0(+0xfc60) [0x7f6ded592c60]
123 /// .../opensim/bin/libode-x86_64.so(_ZN6Opcode11OBBCollider8_CollideEPKNS_14AABBNoLeafNodeE+0xd7a) [0x7f6dd822628a]
124 ///
125 /// ODE provides an experimental option to cache in thread local storage but compiling ODE with this option
126 /// causes OpenSimulator to immediately crash with a native stack trace similar to
127 ///
128 /// mono() [0x489171]
129 /// mono() [0x4d154f]
130 /// /lib/x86_64-linux-gnu/libpthread.so.0(+0xfc60) [0x7f03c9849c60]
131 /// .../opensim/bin/libode-x86_64.so(_Z12dCollideCCTLP6dxGeomS0_iP12dContactGeomi+0x92) [0x7f03b44bcf82]
132 /// </remarks>
133 internal static Object UniversalColliderSyncObject = new Object();
134
135 /// <summary>
136 /// Is stats collecting enabled for this ODE scene?
137 /// </summary>
138 public bool CollectStats { get; set; }
139
140 /// <summary>
141 /// Statistics for this scene.
142 /// </summary>
143 private Dictionary<string, float> m_stats = new Dictionary<string, float>();
144
145 /// <summary>
146 /// Stat name for total number of avatars in this ODE scene.
147 /// </summary>
148 public const string ODETotalAvatarsStatName = "ODETotalAvatars";
149
150 /// <summary>
151 /// Stat name for total number of prims in this ODE scene.
152 /// </summary>
153 public const string ODETotalPrimsStatName = "ODETotalPrims";
154
155 /// <summary>
156 /// Stat name for total number of prims with active physics in this ODE scene.
157 /// </summary>
158 public const string ODEActivePrimsStatName = "ODEActivePrims";
159
160 /// <summary>
161 /// Stat name for the total time spent in ODE frame processing.
162 /// </summary>
163 /// <remarks>
164 /// A sanity check for the main scene loop physics time.
165 /// </remarks>
166 public const string ODETotalFrameMsStatName = "ODETotalFrameMS";
167
168 /// <summary>
169 /// Stat name for time spent processing avatar taints per frame
170 /// </summary>
171 public const string ODEAvatarTaintMsStatName = "ODEAvatarTaintFrameMS";
172
173 /// <summary>
174 /// Stat name for time spent processing prim taints per frame
175 /// </summary>
176 public const string ODEPrimTaintMsStatName = "ODEPrimTaintFrameMS";
177
178 /// <summary>
179 /// Stat name for time spent calculating avatar forces per frame.
180 /// </summary>
181 public const string ODEAvatarForcesFrameMsStatName = "ODEAvatarForcesFrameMS";
182
183 /// <summary>
184 /// Stat name for time spent calculating prim forces per frame
185 /// </summary>
186 public const string ODEPrimForcesFrameMsStatName = "ODEPrimForcesFrameMS";
187
188 /// <summary>
189 /// Stat name for time spent fulfilling raycasting requests per frame
190 /// </summary>
191 public const string ODERaycastingFrameMsStatName = "ODERaycastingFrameMS";
192
193 /// <summary>
194 /// Stat name for time spent in native code that actually steps through the simulation.
195 /// </summary>
196 public const string ODENativeStepFrameMsStatName = "ODENativeStepFrameMS";
197
198 /// <summary>
199 /// Stat name for the number of milliseconds that ODE spends in native space collision code.
200 /// </summary>
201 public const string ODENativeSpaceCollisionFrameMsStatName = "ODENativeSpaceCollisionFrameMS";
202
203 /// <summary>
204 /// Stat name for milliseconds that ODE spends in native geom collision code.
205 /// </summary>
206 public const string ODENativeGeomCollisionFrameMsStatName = "ODENativeGeomCollisionFrameMS";
207
208 /// <summary>
209 /// Time spent in collision processing that is not spent in native space or geom collision code.
210 /// </summary>
211 public const string ODEOtherCollisionFrameMsStatName = "ODEOtherCollisionFrameMS";
212
213 /// <summary>
214 /// Stat name for time spent notifying listeners of collisions
215 /// </summary>
216 public const string ODECollisionNotificationFrameMsStatName = "ODECollisionNotificationFrameMS";
217
218 /// <summary>
219 /// Stat name for milliseconds spent updating avatar position and velocity
220 /// </summary>
221 public const string ODEAvatarUpdateFrameMsStatName = "ODEAvatarUpdateFrameMS";
222
223 /// <summary>
224 /// Stat name for the milliseconds spent updating prim position and velocity
225 /// </summary>
226 public const string ODEPrimUpdateFrameMsStatName = "ODEPrimUpdateFrameMS";
227
228 /// <summary>
229 /// Stat name for avatar collisions with another entity.
230 /// </summary>
231 public const string ODEAvatarContactsStatsName = "ODEAvatarContacts";
232
233 /// <summary>
234 /// Stat name for prim collisions with another entity.
235 /// </summary>
236 public const string ODEPrimContactsStatName = "ODEPrimContacts";
237
238 /// <summary>
239 /// Used to hold tick numbers for stat collection purposes.
240 /// </summary>
241 private int m_nativeCollisionStartTick;
242
243 /// <summary>
244 /// A messy way to tell if we need to avoid adding a collision time because this was already done in the callback.
245 /// </summary>
246 private bool m_inCollisionTiming;
247
248 /// <summary>
249 /// A temporary holder for the number of avatar collisions in a frame, so we can work out how many object
250 /// collisions occured using the _perloopcontact if stats collection is enabled.
251 /// </summary>
252 private int m_tempAvatarCollisionsThisFrame;
253 189
254 /// <summary> 190 const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce;
255 /// Used in calculating physics frame time dilation 191 const float MaxERP = 0.8f;
256 /// </summary> 192 const float minERP = 0.1f;
257 private int tickCountFrameRun; 193 const float comumContactCFM = 0.0001f;
194
195 float frictionMovementMult = 0.8f;
258 196
259 /// <summary> 197 float TerrainBounce = 0.1f;
260 /// Used in calculating physics frame time dilation 198 float TerrainFriction = 0.3f;
261 /// </summary>
262 private int latertickcount;
263 199
264 private Random fluidRandomizer = new Random(Environment.TickCount); 200 public float AvatarFriction = 0;// 0.9f * 0.5f;
265 201
266 private const uint m_regionWidth = Constants.RegionSize; 202 private const uint m_regionWidth = Constants.RegionSize;
267 private const uint m_regionHeight = Constants.RegionSize; 203 private const uint m_regionHeight = Constants.RegionSize;
268 204
269 private float ODE_STEPSIZE = 0.0178f; 205 public float ODE_STEPSIZE = 0.020f;
270 private float metersInSpace = 29.9f; 206 public float HalfOdeStep = 0.01f;
207 public int odetimestepMS = 20; // rounded
208 private float metersInSpace = 25.6f;
271 private float m_timeDilation = 1.0f; 209 private float m_timeDilation = 1.0f;
272 210
211 private DateTime m_lastframe;
212 private DateTime m_lastMeshExpire;
213
273 public float gravityx = 0f; 214 public float gravityx = 0f;
274 public float gravityy = 0f; 215 public float gravityy = 0f;
275 public float gravityz = -9.8f; 216 public float gravityz = -9.8f;
276 217
277 public float AvatarTerminalVelocity { get; set; }
278
279 private float contactsurfacelayer = 0.001f;
280
281 private int worldHashspaceLow = -4;
282 private int worldHashspaceHigh = 128;
283
284 private int smallHashspaceLow = -4;
285 private int smallHashspaceHigh = 66;
286
287 private float waterlevel = 0f; 218 private float waterlevel = 0f;
288 private int framecount = 0; 219 private int framecount = 0;
289 //private int m_returncollisions = 10;
290
291 private readonly IntPtr contactgroup;
292
293 internal IntPtr WaterGeom;
294 220
295 private float nmTerrainContactFriction = 255.0f; 221 private int m_meshExpireCntr;
296 private float nmTerrainContactBounce = 0.1f;
297 private float nmTerrainContactERP = 0.1025f;
298 222
299 private float mTerrainContactFriction = 75f; 223// private IntPtr WaterGeom = IntPtr.Zero;
300 private float mTerrainContactBounce = 0.1f; 224// private IntPtr WaterHeightmapData = IntPtr.Zero;
301 private float mTerrainContactERP = 0.05025f; 225// private GCHandle WaterMapHandler = new GCHandle();
302 226
303 private float nmAvatarObjectContactFriction = 250f; 227 public float avPIDD = 2200f; // make it visible
304 private float nmAvatarObjectContactBounce = 0.1f; 228 public float avPIDP = 900f; // make it visible
305
306 private float mAvatarObjectContactFriction = 75f;
307 private float mAvatarObjectContactBounce = 0.1f;
308
309 private float avPIDD = 3200f;
310 private float avPIDP = 1400f;
311 private float avCapRadius = 0.37f; 229 private float avCapRadius = 0.37f;
312 private float avStandupTensor = 2000000f; 230 private float avDensity = 3f;
313
314 /// <summary>
315 /// true = old compatibility mode with leaning capsule; false = new corrected mode
316 /// </summary>
317 /// <remarks>
318 /// Even when set to false, the capsule still tilts but this is done in a different way.
319 /// </remarks>
320 public bool IsAvCapsuleTilted { get; private set; }
321
322 private float avDensity = 80f;
323// private float avHeightFudgeFactor = 0.52f;
324 private float avMovementDivisorWalk = 1.3f; 231 private float avMovementDivisorWalk = 1.3f;
325 private float avMovementDivisorRun = 0.8f; 232 private float avMovementDivisorRun = 0.8f;
326 private float minimumGroundFlightOffset = 3f; 233 private float minimumGroundFlightOffset = 3f;
327 public float maximumMassObject = 10000.01f; 234 public float maximumMassObject = 10000.01f;
328 235
329 public bool meshSculptedPrim = true;
330 public bool forceSimplePrimMeshing = false;
331
332 public float meshSculptLOD = 32;
333 public float MeshSculptphysicalLOD = 16;
334 236
335 public float geomDefaultDensity = 10.000006836f; 237 public float geomDefaultDensity = 10.000006836f;
336 238
337 public int geomContactPointsStartthrottle = 3; 239 public int geomContactPointsStartthrottle = 3;
338 public int geomUpdatesPerThrottledUpdate = 15; 240 public int geomUpdatesPerThrottledUpdate = 15;
339 private const int avatarExpectedContacts = 3;
340 241
341 public float bodyPIDD = 35f; 242 public float bodyPIDD = 35f;
342 public float bodyPIDG = 25; 243 public float bodyPIDG = 25;
343 244
344 public int geomCrossingFailuresBeforeOutofbounds = 5; 245// public int geomCrossingFailuresBeforeOutofbounds = 6;
345
346 public float bodyMotorJointMaxforceTensor = 2;
347 246
348 public int bodyFramesAutoDisable = 20; 247 public int bodyFramesAutoDisable = 5;
349 248
350 private float[] _watermap;
351 private bool m_filterCollisions = true;
352 249
353 private d.NearCallback nearCallback; 250 private d.NearCallback nearCallback;
354 public d.TriCallback triCallback;
355 public d.TriArrayCallback triArrayCallback;
356 251
357 /// <summary> 252 private HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>();
358 /// Avatars in the physics scene. 253 private HashSet<OdePrim> _prims = new HashSet<OdePrim>();
359 /// </summary> 254 private HashSet<OdePrim> _activeprims = new HashSet<OdePrim>();
360 private readonly HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>(); 255 private HashSet<OdePrim> _activegroups = new HashSet<OdePrim>();
361 256
362 /// <summary> 257 public OpenSim.Framework.LocklessQueue<ODEchangeitem> ChangesQueue = new OpenSim.Framework.LocklessQueue<ODEchangeitem>();
363 /// Prims in the physics scene.
364 /// </summary>
365 private readonly HashSet<OdePrim> _prims = new HashSet<OdePrim>();
366 258
367 /// <summary> 259 /// <summary>
368 /// Prims in the physics scene that are subject to physics, not just collisions. 260 /// A list of actors that should receive collision events.
369 /// </summary> 261 /// </summary>
370 private readonly HashSet<OdePrim> _activeprims = new HashSet<OdePrim>(); 262 private List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>();
371 263 private List<PhysicsActor> _collisionEventPrimRemove = new List<PhysicsActor>();
372 /// <summary> 264
373 /// Prims that the simulator has created/deleted/updated and so need updating in ODE. 265 private HashSet<OdeCharacter> _badCharacter = new HashSet<OdeCharacter>();
374 /// </summary> 266// public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>();
375 private readonly HashSet<OdePrim> _taintedPrims = new HashSet<OdePrim>(); 267 public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>();
376
377 /// <summary>
378 /// Record a character that has taints to be processed.
379 /// </summary>
380 private readonly HashSet<OdeCharacter> _taintedActors = new HashSet<OdeCharacter>();
381 268
382 /// <summary> 269 private float contactsurfacelayer = 0.002f;
383 /// Keep record of contacts in the physics loop so that we can remove duplicates.
384 /// </summary>
385 private readonly List<d.ContactGeom> _perloopContact = new List<d.ContactGeom>();
386 270
387 /// <summary> 271 private int contactsPerCollision = 80;
388 /// A dictionary of actors that should receive collision events. 272 internal IntPtr ContactgeomsArray = IntPtr.Zero;
389 /// </summary> 273 private IntPtr GlobalContactsArray = IntPtr.Zero;
390 private readonly Dictionary<uint, PhysicsActor> m_collisionEventActors = new Dictionary<uint, PhysicsActor>();
391 274
392 /// <summary> 275 const int maxContactsbeforedeath = 4000;
393 /// A dictionary of collision event changes that are waiting to be processed. 276 private volatile int m_global_contactcount = 0;
394 /// </summary>
395 private readonly Dictionary<uint, PhysicsActor> m_collisionEventActorsChanges = new Dictionary<uint, PhysicsActor>();
396 277
397 /// <summary> 278 private IntPtr contactgroup;
398 /// Maps a unique geometry id (a memory location) to a physics actor name.
399 /// </summary>
400 /// <remarks>
401 /// Only actors participating in collisions have geometries. This has to be maintained separately from
402 /// actor_name_map because terrain and water currently don't conceptually have a physics actor of their own
403 /// apart from the singleton PANull
404 /// </remarks>
405 public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>();
406 279
407 /// <summary> 280 public ContactData[] m_materialContactsData = new ContactData[8];
408 /// Maps a unique geometry id (a memory location) to a physics actor.
409 /// </summary>
410 /// <remarks>
411 /// Only actors participating in collisions have geometries.
412 /// </remarks>
413 public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>();
414 281
415 /// <summary> 282 private Dictionary<Vector3, IntPtr> RegionTerrain = new Dictionary<Vector3, IntPtr>();
416 /// Defects list to remove characters that no longer have finite positions due to some other bug. 283 private Dictionary<IntPtr, float[]> TerrainHeightFieldHeights = new Dictionary<IntPtr, float[]>();
417 /// </summary> 284 private Dictionary<IntPtr, GCHandle> TerrainHeightFieldHeightsHandlers = new Dictionary<IntPtr, GCHandle>();
418 /// <remarks> 285
419 /// Used repeatedly in Simulate() but initialized once here. 286 private int m_physicsiterations = 10;
420 /// </remarks> 287 private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag
421 private readonly List<OdeCharacter> defects = new List<OdeCharacter>(); 288// private PhysicsActor PANull = new NullPhysicsActor();
289 private float step_time = 0.0f;
422 290
423 private bool m_NINJA_physics_joints_enabled = false; 291 public IntPtr world;
424 //private Dictionary<String, IntPtr> jointpart_name_map = new Dictionary<String,IntPtr>();
425 private readonly Dictionary<String, List<PhysicsJoint>> joints_connecting_actor = new Dictionary<String, List<PhysicsJoint>>();
426 private d.ContactGeom[] contacts;
427 292
428 /// <summary>
429 /// Lock only briefly. accessed by external code (to request new joints) and by OdeScene.Simulate() to move those joints into pending/active
430 /// </summary>
431 private readonly List<PhysicsJoint> requestedJointsToBeCreated = new List<PhysicsJoint>();
432 293
433 /// <summary> 294 // split the spaces acording to contents type
434 /// can lock for longer. accessed only by OdeScene. 295 // ActiveSpace contains characters and active prims
435 /// </summary> 296 // StaticSpace contains land and other that is mostly static in enviroment
436 private readonly List<PhysicsJoint> pendingJoints = new List<PhysicsJoint>(); 297 // this can contain subspaces, like the grid in staticspace
298 // as now space only contains this 2 top spaces
437 299
438 /// <summary> 300 public IntPtr TopSpace; // the global space
439 /// can lock for longer. accessed only by OdeScene. 301 public IntPtr ActiveSpace; // space for active prims
440 /// </summary> 302 public IntPtr StaticSpace; // space for the static things around
441 private readonly List<PhysicsJoint> activeJoints = new List<PhysicsJoint>(); 303 public IntPtr GroundSpace; // space for ground
442 304
443 /// <summary> 305 // some speedup variables
444 /// lock only briefly. accessed by external code (to request deletion of joints) and by OdeScene.Simulate() to move those joints out of pending/active 306 private int spaceGridMaxX;
445 /// </summary> 307 private int spaceGridMaxY;
446 private readonly List<string> requestedJointsToBeDeleted = new List<string>(); 308 private float spacesPerMeter;
447
448 private Object externalJointRequestsLock = new Object();
449 private readonly Dictionary<String, PhysicsJoint> SOPName_to_activeJoint = new Dictionary<String, PhysicsJoint>();
450 private readonly Dictionary<String, PhysicsJoint> SOPName_to_pendingJoint = new Dictionary<String, PhysicsJoint>();
451 private readonly DoubleDictionary<Vector3, IntPtr, IntPtr> RegionTerrain = new DoubleDictionary<Vector3, IntPtr, IntPtr>();
452 private readonly Dictionary<IntPtr,float[]> TerrainHeightFieldHeights = new Dictionary<IntPtr, float[]>();
453
454 private d.Contact contact;
455 private d.Contact TerrainContact;
456 private d.Contact AvatarMovementprimContact;
457 private d.Contact AvatarMovementTerrainContact;
458 private d.Contact WaterContact;
459 private d.Contact[,] m_materialContacts;
460
461//Ckrinke: Comment out until used. We declare it, initialize it, but do not use it
462//Ckrinke private int m_randomizeWater = 200;
463 private int m_physicsiterations = 10;
464 private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag
465 private readonly PhysicsActor PANull = new NullPhysicsActor();
466// private float step_time = 0.0f;
467//Ckrinke: Comment out until used. We declare it, initialize it, but do not use it
468//Ckrinke private int ms = 0;
469 public IntPtr world;
470 //private bool returncollisions = false;
471 // private uint obj1LocalID = 0;
472 private uint obj2LocalID = 0;
473 //private int ctype = 0;
474 private OdeCharacter cc1;
475 private OdePrim cp1;
476 private OdeCharacter cc2;
477 private OdePrim cp2;
478 private int p1ExpectedPoints = 0;
479 private int p2ExpectedPoints = 0;
480 //private int cStartStop = 0;
481 //private string cDictKey = "";
482
483 public IntPtr space;
484
485 //private IntPtr tmpSpace;
486 // split static geometry collision handling into spaces of 30 meters
487 public IntPtr[,] staticPrimspace;
488 309
489 /// <summary> 310 // split static geometry collision into a grid as before
490 /// Used to lock the entire physics scene. Locked during the main part of Simulate() 311 private IntPtr[,] staticPrimspace;
491 /// </summary> 312 private IntPtr[] staticPrimspaceOffRegion;
492 internal Object OdeLock = new Object();
493 313
494 private bool _worldInitialized = false; 314 public Object OdeLock;
315 public static Object SimulationLock;
495 316
496 public IMesher mesher; 317 public IMesher mesher;
497 318
@@ -501,461 +322,340 @@ namespace OpenSim.Region.Physics.OdePlugin
501 public int physics_logging_interval = 0; 322 public int physics_logging_interval = 0;
502 public bool physics_logging_append_existing_logfile = false; 323 public bool physics_logging_append_existing_logfile = false;
503 324
504
505 public d.Vector3 xyz = new d.Vector3(128.1640f, 128.3079f, 25.7600f);
506 public d.Vector3 hpr = new d.Vector3(125.5000f, -17.0000f, 0.0000f);
507
508 // TODO: unused: private uint heightmapWidth = m_regionWidth + 1;
509 // TODO: unused: private uint heightmapHeight = m_regionHeight + 1;
510 // TODO: unused: private uint heightmapWidthSamples;
511 // TODO: unused: private uint heightmapHeightSamples;
512
513 private volatile int m_global_contactcount = 0;
514
515 private Vector3 m_worldOffset = Vector3.Zero; 325 private Vector3 m_worldOffset = Vector3.Zero;
516 public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); 326 public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
517 private PhysicsScene m_parentScene = null; 327 private PhysicsScene m_parentScene = null;
518 328
519 private ODERayCastRequestManager m_rayCastManager; 329 private ODERayCastRequestManager m_rayCastManager;
330 public ODEMeshWorker m_meshWorker;
331
332/* maybe needed if ode uses tls
333 private void checkThread()
334 {
520 335
336 int th = Thread.CurrentThread.ManagedThreadId;
337 if(th != threadid)
338 {
339 threadid = th;
340 d.AllocateODEDataForThread(~0U);
341 }
342 }
343 */
521 /// <summary> 344 /// <summary>
522 /// Initiailizes the scene 345 /// Initiailizes the scene
523 /// Sets many properties that ODE requires to be stable 346 /// Sets many properties that ODE requires to be stable
524 /// These settings need to be tweaked 'exactly' right or weird stuff happens. 347 /// These settings need to be tweaked 'exactly' right or weird stuff happens.
525 /// </summary> 348 /// </summary>
526 /// <param value="name">Name of the scene. Useful in debug messages.</param> 349 public OdeScene(string sceneIdentifier)
527 public OdeScene(string name) 350 {
528 { 351 m_log
529 m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + name); 352 = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + sceneIdentifier);
530 353
531 Name = name; 354// checkThread();
355 Name = sceneIdentifier;
356
357 OdeLock = new Object();
358 SimulationLock = new Object();
532 359
533 nearCallback = near; 360 nearCallback = near;
534 triCallback = TriCallback; 361
535 triArrayCallback = TriArrayCallback;
536 m_rayCastManager = new ODERayCastRequestManager(this); 362 m_rayCastManager = new ODERayCastRequestManager(this);
363
537 364
538 // Create the world and the first space 365 lock (OdeLock)
539 world = d.WorldCreate(); 366 {
540 space = d.HashSpaceCreate(IntPtr.Zero); 367 // Create the world and the first space
368 try
369 {
370 world = d.WorldCreate();
371 TopSpace = d.HashSpaceCreate(IntPtr.Zero);
541 372
542 contactgroup = d.JointGroupCreate(0); 373 // now the major subspaces
374 ActiveSpace = d.HashSpaceCreate(TopSpace);
375 StaticSpace = d.HashSpaceCreate(TopSpace);
376 GroundSpace = d.HashSpaceCreate(TopSpace);
377 }
378 catch
379 {
380 // i must RtC#FM
381 }
543 382
544 d.WorldSetAutoDisableFlag(world, false); 383 d.HashSpaceSetLevels(TopSpace, -2, 8);
384 d.HashSpaceSetLevels(ActiveSpace, -2, 8);
385 d.HashSpaceSetLevels(StaticSpace, -2, 8);
386 d.HashSpaceSetLevels(GroundSpace, 0, 8);
545 387
546 #if USE_DRAWSTUFF 388 // demote to second level
547 Thread viewthread = new Thread(new ParameterizedThreadStart(startvisualization)); 389 d.SpaceSetSublevel(ActiveSpace, 1);
548 viewthread.Start(); 390 d.SpaceSetSublevel(StaticSpace, 1);
549 #endif 391 d.SpaceSetSublevel(GroundSpace, 1);
550 392
551 _watermap = new float[258 * 258]; 393 d.GeomSetCategoryBits(ActiveSpace, (uint)(CollisionCategories.Space |
394 CollisionCategories.Geom |
395 CollisionCategories.Character |
396 CollisionCategories.Phantom |
397 CollisionCategories.VolumeDtc
398 ));
399 d.GeomSetCollideBits(ActiveSpace, 0);
400 d.GeomSetCategoryBits(StaticSpace, (uint)(CollisionCategories.Space |
401 CollisionCategories.Geom |
402 CollisionCategories.Land |
403 CollisionCategories.Water |
404 CollisionCategories.Phantom |
405 CollisionCategories.VolumeDtc
406 ));
407 d.GeomSetCollideBits(StaticSpace, 0);
552 408
553 // Zero out the prim spaces array (we split our space into smaller spaces so 409 d.GeomSetCategoryBits(GroundSpace, (uint)(CollisionCategories.Land));
554 // we can hit test less. 410 d.GeomSetCollideBits(GroundSpace, 0);
555 }
556 411
557#if USE_DRAWSTUFF 412 contactgroup = d.JointGroupCreate(0);
558 public void startvisualization(object o) 413 //contactgroup
559 { 414
560 ds.Functions fn; 415 d.WorldSetAutoDisableFlag(world, false);
561 fn.version = ds.VERSION; 416 }
562 fn.start = new ds.CallbackFunction(start);
563 fn.step = new ds.CallbackFunction(step);
564 fn.command = new ds.CallbackFunction(command);
565 fn.stop = null;
566 fn.path_to_textures = "./textures";
567 string[] args = new string[0];
568 ds.SimulationLoop(args.Length, args, 352, 288, ref fn);
569 } 417 }
570#endif
571 418
572 // Initialize the mesh plugin 419 // Initialize the mesh plugin
420// public override void Initialise(IMesher meshmerizer, IConfigSource config, RegionInfo region )
573 public override void Initialise(IMesher meshmerizer, IConfigSource config) 421 public override void Initialise(IMesher meshmerizer, IConfigSource config)
574 { 422 {
575 InitializeExtraStats(); 423// checkThread();
576
577 mesher = meshmerizer; 424 mesher = meshmerizer;
578 m_config = config; 425 m_config = config;
579 // Defaults
580 426
581 if (Environment.OSVersion.Platform == PlatformID.Unix) 427 string ode_config = d.GetConfiguration();
428 if (ode_config != null && ode_config != "")
582 { 429 {
583 avPIDD = 3200.0f; 430 m_log.WarnFormat("ODE configuration: {0}", ode_config);
584 avPIDP = 1400.0f; 431
585 avStandupTensor = 2000000f; 432 if (ode_config.Contains("ODE_Ubit"))
586 } 433 {
587 else 434 OdeUbitLib = true;
588 { 435 }
589 avPIDD = 2200.0f;
590 avPIDP = 900.0f;
591 avStandupTensor = 550000f;
592 } 436 }
593 437
438 /*
439 if (region != null)
440 {
441 WorldExtents.X = region.RegionSizeX;
442 WorldExtents.Y = region.RegionSizeY;
443 }
444 */
445
446 // Defaults
447
594 int contactsPerCollision = 80; 448 int contactsPerCollision = 80;
595 449
450 IConfig physicsconfig = null;
451
596 if (m_config != null) 452 if (m_config != null)
597 { 453 {
598 IConfig physicsconfig = m_config.Configs["ODEPhysicsSettings"]; 454 physicsconfig = m_config.Configs["ODEPhysicsSettings"];
599 if (physicsconfig != null) 455 if (physicsconfig != null)
600 { 456 {
601 CollectStats = physicsconfig.GetBoolean("collect_stats", false); 457 gravityx = physicsconfig.GetFloat("world_gravityx", gravityx);
602 458 gravityy = physicsconfig.GetFloat("world_gravityy", gravityy);
603 gravityx = physicsconfig.GetFloat("world_gravityx", 0f); 459 gravityz = physicsconfig.GetFloat("world_gravityz", gravityz);
604 gravityy = physicsconfig.GetFloat("world_gravityy", 0f);
605 gravityz = physicsconfig.GetFloat("world_gravityz", -9.8f);
606
607 float avatarTerminalVelocity = physicsconfig.GetFloat("avatar_terminal_velocity", 54f);
608 AvatarTerminalVelocity = Util.Clamp<float>(avatarTerminalVelocity, 0, 255f);
609 if (AvatarTerminalVelocity != avatarTerminalVelocity)
610 {
611 m_log.WarnFormat(
612 "[ODE SCENE]: avatar_terminal_velocity of {0} is invalid. Clamping to {1}",
613 avatarTerminalVelocity, AvatarTerminalVelocity);
614 }
615
616 worldHashspaceLow = physicsconfig.GetInt("world_hashspace_size_low", -4);
617 worldHashspaceHigh = physicsconfig.GetInt("world_hashspace_size_high", 128);
618
619 metersInSpace = physicsconfig.GetFloat("meters_in_small_space", 29.9f);
620 smallHashspaceLow = physicsconfig.GetInt("small_hashspace_size_low", -4);
621 smallHashspaceHigh = physicsconfig.GetInt("small_hashspace_size_high", 66);
622
623 contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", 0.001f);
624
625 nmTerrainContactFriction = physicsconfig.GetFloat("nm_terraincontact_friction", 255.0f);
626 nmTerrainContactBounce = physicsconfig.GetFloat("nm_terraincontact_bounce", 0.1f);
627 nmTerrainContactERP = physicsconfig.GetFloat("nm_terraincontact_erp", 0.1025f);
628 460
629 mTerrainContactFriction = physicsconfig.GetFloat("m_terraincontact_friction", 75f); 461 metersInSpace = physicsconfig.GetFloat("meters_in_small_space", metersInSpace);
630 mTerrainContactBounce = physicsconfig.GetFloat("m_terraincontact_bounce", 0.05f);
631 mTerrainContactERP = physicsconfig.GetFloat("m_terraincontact_erp", 0.05025f);
632 462
633 nmAvatarObjectContactFriction = physicsconfig.GetFloat("objectcontact_friction", 250f); 463 contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer);
634 nmAvatarObjectContactBounce = physicsconfig.GetFloat("objectcontact_bounce", 0.2f);
635
636 mAvatarObjectContactFriction = physicsconfig.GetFloat("m_avatarobjectcontact_friction", 75f);
637 mAvatarObjectContactBounce = physicsconfig.GetFloat("m_avatarobjectcontact_bounce", 0.1f);
638 464
639 ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE); 465 ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE);
640 m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", 10); 466 m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", m_physicsiterations);
641 467
642 avDensity = physicsconfig.GetFloat("av_density", 80f); 468 avDensity = physicsconfig.GetFloat("av_density", avDensity);
643// avHeightFudgeFactor = physicsconfig.GetFloat("av_height_fudge_factor", 0.52f); 469 avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk);
644 avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", 1.3f); 470 avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", avMovementDivisorRun);
645 avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", 0.8f); 471 avCapRadius = physicsconfig.GetFloat("av_capsule_radius", avCapRadius);
646 avCapRadius = physicsconfig.GetFloat("av_capsule_radius", 0.37f);
647 IsAvCapsuleTilted = physicsconfig.GetBoolean("av_capsule_tilted", false);
648 472
649 contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", 80); 473 contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision);
650 474
651 geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 5); 475 geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3);
652 geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15); 476 geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15);
653 geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5); 477// geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5);
654
655 geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", 10.000006836f);
656 bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", 20);
657
658 bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", 35f);
659 bodyPIDG = physicsconfig.GetFloat("body_pid_gain", 25f);
660 478
661 forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing); 479 geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity);
662 meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", true); 480 bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable);
663 meshSculptLOD = physicsconfig.GetFloat("mesh_lod", 32f);
664 MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", 16f);
665 m_filterCollisions = physicsconfig.GetBoolean("filter_collisions", false);
666
667 if (Environment.OSVersion.Platform == PlatformID.Unix)
668 {
669 avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", 2200.0f);
670 avPIDP = physicsconfig.GetFloat("av_pid_proportional_linux", 900.0f);
671 avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor_linux", 550000f);
672 bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_linux", 5f);
673 }
674 else
675 {
676 avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", 2200.0f);
677 avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", 900.0f);
678 avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor_win", 550000f);
679 bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_win", 5f);
680 }
681 481
682 physics_logging = physicsconfig.GetBoolean("physics_logging", false); 482 physics_logging = physicsconfig.GetBoolean("physics_logging", false);
683 physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0); 483 physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0);
684 physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false); 484 physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false);
685 485
686 m_NINJA_physics_joints_enabled = physicsconfig.GetBoolean("use_NINJA_physics_joints", false); 486 minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", minimumGroundFlightOffset);
687 minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", 3f); 487 maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", maximumMassObject);
688 maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", 10000.01f);
689 } 488 }
690 } 489 }
691 490
692 contacts = new d.ContactGeom[contactsPerCollision]; 491 m_meshWorker = new ODEMeshWorker(this, m_log, meshmerizer, physicsconfig);
693
694 staticPrimspace = new IntPtr[(int)(300 / metersInSpace), (int)(300 / metersInSpace)];
695
696 // Centeral contact friction and bounce
697 // ckrinke 11/10/08 Enabling soft_erp but not soft_cfm until I figure out why
698 // an avatar falls through in Z but not in X or Y when walking on a prim.
699 contact.surface.mode |= d.ContactFlags.SoftERP;
700 contact.surface.mu = nmAvatarObjectContactFriction;
701 contact.surface.bounce = nmAvatarObjectContactBounce;
702 contact.surface.soft_cfm = 0.010f;
703 contact.surface.soft_erp = 0.010f;
704
705 // Terrain contact friction and Bounce
706 // This is the *non* moving version. Use this when an avatar
707 // isn't moving to keep it in place better
708 TerrainContact.surface.mode |= d.ContactFlags.SoftERP;
709 TerrainContact.surface.mu = nmTerrainContactFriction;
710 TerrainContact.surface.bounce = nmTerrainContactBounce;
711 TerrainContact.surface.soft_erp = nmTerrainContactERP;
712
713 WaterContact.surface.mode |= (d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM);
714 WaterContact.surface.mu = 0f; // No friction
715 WaterContact.surface.bounce = 0.0f; // No bounce
716 WaterContact.surface.soft_cfm = 0.010f;
717 WaterContact.surface.soft_erp = 0.010f;
718
719 // Prim contact friction and bounce
720 // THis is the *non* moving version of friction and bounce
721 // Use this when an avatar comes in contact with a prim
722 // and is moving
723 AvatarMovementprimContact.surface.mu = mAvatarObjectContactFriction;
724 AvatarMovementprimContact.surface.bounce = mAvatarObjectContactBounce;
725
726 // Terrain contact friction bounce and various error correcting calculations
727 // Use this when an avatar is in contact with the terrain and moving.
728 AvatarMovementTerrainContact.surface.mode |= d.ContactFlags.SoftERP;
729 AvatarMovementTerrainContact.surface.mu = mTerrainContactFriction;
730 AvatarMovementTerrainContact.surface.bounce = mTerrainContactBounce;
731 AvatarMovementTerrainContact.surface.soft_erp = mTerrainContactERP;
732 492
733 /* 493 HalfOdeStep = ODE_STEPSIZE * 0.5f;
734 <summary></summary> 494 odetimestepMS = (int)(1000.0f * ODE_STEPSIZE +0.5f);
735 Stone = 0,
736 /// <summary></summary>
737 Metal = 1,
738 /// <summary></summary>
739 Glass = 2,
740 /// <summary></summary>
741 Wood = 3,
742 /// <summary></summary>
743 Flesh = 4,
744 /// <summary></summary>
745 Plastic = 5,
746 /// <summary></summary>
747 Rubber = 6
748 */
749 495
750 m_materialContacts = new d.Contact[7,2]; 496 ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf);
751 497 GlobalContactsArray = GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf);
752 m_materialContacts[(int)Material.Stone, 0] = new d.Contact();
753 m_materialContacts[(int)Material.Stone, 0].surface.mode |= d.ContactFlags.SoftERP;
754 m_materialContacts[(int)Material.Stone, 0].surface.mu = nmAvatarObjectContactFriction;
755 m_materialContacts[(int)Material.Stone, 0].surface.bounce = nmAvatarObjectContactBounce;
756 m_materialContacts[(int)Material.Stone, 0].surface.soft_cfm = 0.010f;
757 m_materialContacts[(int)Material.Stone, 0].surface.soft_erp = 0.010f;
758
759 m_materialContacts[(int)Material.Stone, 1] = new d.Contact();
760 m_materialContacts[(int)Material.Stone, 1].surface.mode |= d.ContactFlags.SoftERP;
761 m_materialContacts[(int)Material.Stone, 1].surface.mu = mAvatarObjectContactFriction;
762 m_materialContacts[(int)Material.Stone, 1].surface.bounce = mAvatarObjectContactBounce;
763 m_materialContacts[(int)Material.Stone, 1].surface.soft_cfm = 0.010f;
764 m_materialContacts[(int)Material.Stone, 1].surface.soft_erp = 0.010f;
765
766 m_materialContacts[(int)Material.Metal, 0] = new d.Contact();
767 m_materialContacts[(int)Material.Metal, 0].surface.mode |= d.ContactFlags.SoftERP;
768 m_materialContacts[(int)Material.Metal, 0].surface.mu = nmAvatarObjectContactFriction;
769 m_materialContacts[(int)Material.Metal, 0].surface.bounce = nmAvatarObjectContactBounce;
770 m_materialContacts[(int)Material.Metal, 0].surface.soft_cfm = 0.010f;
771 m_materialContacts[(int)Material.Metal, 0].surface.soft_erp = 0.010f;
772
773 m_materialContacts[(int)Material.Metal, 1] = new d.Contact();
774 m_materialContacts[(int)Material.Metal, 1].surface.mode |= d.ContactFlags.SoftERP;
775 m_materialContacts[(int)Material.Metal, 1].surface.mu = mAvatarObjectContactFriction;
776 m_materialContacts[(int)Material.Metal, 1].surface.bounce = mAvatarObjectContactBounce;
777 m_materialContacts[(int)Material.Metal, 1].surface.soft_cfm = 0.010f;
778 m_materialContacts[(int)Material.Metal, 1].surface.soft_erp = 0.010f;
779
780 m_materialContacts[(int)Material.Glass, 0] = new d.Contact();
781 m_materialContacts[(int)Material.Glass, 0].surface.mode |= d.ContactFlags.SoftERP;
782 m_materialContacts[(int)Material.Glass, 0].surface.mu = 1f;
783 m_materialContacts[(int)Material.Glass, 0].surface.bounce = 0.5f;
784 m_materialContacts[(int)Material.Glass, 0].surface.soft_cfm = 0.010f;
785 m_materialContacts[(int)Material.Glass, 0].surface.soft_erp = 0.010f;
786 498
787 /* 499 m_materialContactsData[(int)Material.Stone].mu = 0.8f;
788 private float nmAvatarObjectContactFriction = 250f; 500 m_materialContactsData[(int)Material.Stone].bounce = 0.4f;
789 private float nmAvatarObjectContactBounce = 0.1f;
790 501
791 private float mAvatarObjectContactFriction = 75f; 502 m_materialContactsData[(int)Material.Metal].mu = 0.3f;
792 private float mAvatarObjectContactBounce = 0.1f; 503 m_materialContactsData[(int)Material.Metal].bounce = 0.4f;
793 */ 504
794 m_materialContacts[(int)Material.Glass, 1] = new d.Contact(); 505 m_materialContactsData[(int)Material.Glass].mu = 0.2f;
795 m_materialContacts[(int)Material.Glass, 1].surface.mode |= d.ContactFlags.SoftERP; 506 m_materialContactsData[(int)Material.Glass].bounce = 0.7f;
796 m_materialContacts[(int)Material.Glass, 1].surface.mu = 1f; 507
797 m_materialContacts[(int)Material.Glass, 1].surface.bounce = 0.5f; 508 m_materialContactsData[(int)Material.Wood].mu = 0.6f;
798 m_materialContacts[(int)Material.Glass, 1].surface.soft_cfm = 0.010f; 509 m_materialContactsData[(int)Material.Wood].bounce = 0.5f;
799 m_materialContacts[(int)Material.Glass, 1].surface.soft_erp = 0.010f; 510
800 511 m_materialContactsData[(int)Material.Flesh].mu = 0.9f;
801 m_materialContacts[(int)Material.Wood, 0] = new d.Contact(); 512 m_materialContactsData[(int)Material.Flesh].bounce = 0.3f;
802 m_materialContacts[(int)Material.Wood, 0].surface.mode |= d.ContactFlags.SoftERP; 513
803 m_materialContacts[(int)Material.Wood, 0].surface.mu = nmAvatarObjectContactFriction; 514 m_materialContactsData[(int)Material.Plastic].mu = 0.4f;
804 m_materialContacts[(int)Material.Wood, 0].surface.bounce = nmAvatarObjectContactBounce; 515 m_materialContactsData[(int)Material.Plastic].bounce = 0.7f;
805 m_materialContacts[(int)Material.Wood, 0].surface.soft_cfm = 0.010f; 516
806 m_materialContacts[(int)Material.Wood, 0].surface.soft_erp = 0.010f; 517 m_materialContactsData[(int)Material.Rubber].mu = 0.9f;
807 518 m_materialContactsData[(int)Material.Rubber].bounce = 0.95f;
808 m_materialContacts[(int)Material.Wood, 1] = new d.Contact(); 519
809 m_materialContacts[(int)Material.Wood, 1].surface.mode |= d.ContactFlags.SoftERP; 520 m_materialContactsData[(int)Material.light].mu = 0.0f;
810 m_materialContacts[(int)Material.Wood, 1].surface.mu = mAvatarObjectContactFriction; 521 m_materialContactsData[(int)Material.light].bounce = 0.0f;
811 m_materialContacts[(int)Material.Wood, 1].surface.bounce = mAvatarObjectContactBounce;
812 m_materialContacts[(int)Material.Wood, 1].surface.soft_cfm = 0.010f;
813 m_materialContacts[(int)Material.Wood, 1].surface.soft_erp = 0.010f;
814
815 m_materialContacts[(int)Material.Flesh, 0] = new d.Contact();
816 m_materialContacts[(int)Material.Flesh, 0].surface.mode |= d.ContactFlags.SoftERP;
817 m_materialContacts[(int)Material.Flesh, 0].surface.mu = nmAvatarObjectContactFriction;
818 m_materialContacts[(int)Material.Flesh, 0].surface.bounce = nmAvatarObjectContactBounce;
819 m_materialContacts[(int)Material.Flesh, 0].surface.soft_cfm = 0.010f;
820 m_materialContacts[(int)Material.Flesh, 0].surface.soft_erp = 0.010f;
821
822 m_materialContacts[(int)Material.Flesh, 1] = new d.Contact();
823 m_materialContacts[(int)Material.Flesh, 1].surface.mode |= d.ContactFlags.SoftERP;
824 m_materialContacts[(int)Material.Flesh, 1].surface.mu = mAvatarObjectContactFriction;
825 m_materialContacts[(int)Material.Flesh, 1].surface.bounce = mAvatarObjectContactBounce;
826 m_materialContacts[(int)Material.Flesh, 1].surface.soft_cfm = 0.010f;
827 m_materialContacts[(int)Material.Flesh, 1].surface.soft_erp = 0.010f;
828
829 m_materialContacts[(int)Material.Plastic, 0] = new d.Contact();
830 m_materialContacts[(int)Material.Plastic, 0].surface.mode |= d.ContactFlags.SoftERP;
831 m_materialContacts[(int)Material.Plastic, 0].surface.mu = nmAvatarObjectContactFriction;
832 m_materialContacts[(int)Material.Plastic, 0].surface.bounce = nmAvatarObjectContactBounce;
833 m_materialContacts[(int)Material.Plastic, 0].surface.soft_cfm = 0.010f;
834 m_materialContacts[(int)Material.Plastic, 0].surface.soft_erp = 0.010f;
835
836 m_materialContacts[(int)Material.Plastic, 1] = new d.Contact();
837 m_materialContacts[(int)Material.Plastic, 1].surface.mode |= d.ContactFlags.SoftERP;
838 m_materialContacts[(int)Material.Plastic, 1].surface.mu = mAvatarObjectContactFriction;
839 m_materialContacts[(int)Material.Plastic, 1].surface.bounce = mAvatarObjectContactBounce;
840 m_materialContacts[(int)Material.Plastic, 1].surface.soft_cfm = 0.010f;
841 m_materialContacts[(int)Material.Plastic, 1].surface.soft_erp = 0.010f;
842
843 m_materialContacts[(int)Material.Rubber, 0] = new d.Contact();
844 m_materialContacts[(int)Material.Rubber, 0].surface.mode |= d.ContactFlags.SoftERP;
845 m_materialContacts[(int)Material.Rubber, 0].surface.mu = nmAvatarObjectContactFriction;
846 m_materialContacts[(int)Material.Rubber, 0].surface.bounce = nmAvatarObjectContactBounce;
847 m_materialContacts[(int)Material.Rubber, 0].surface.soft_cfm = 0.010f;
848 m_materialContacts[(int)Material.Rubber, 0].surface.soft_erp = 0.010f;
849
850 m_materialContacts[(int)Material.Rubber, 1] = new d.Contact();
851 m_materialContacts[(int)Material.Rubber, 1].surface.mode |= d.ContactFlags.SoftERP;
852 m_materialContacts[(int)Material.Rubber, 1].surface.mu = mAvatarObjectContactFriction;
853 m_materialContacts[(int)Material.Rubber, 1].surface.bounce = mAvatarObjectContactBounce;
854 m_materialContacts[(int)Material.Rubber, 1].surface.soft_cfm = 0.010f;
855 m_materialContacts[(int)Material.Rubber, 1].surface.soft_erp = 0.010f;
856
857 d.HashSpaceSetLevels(space, worldHashspaceLow, worldHashspaceHigh);
858 522
859 // Set the gravity,, don't disable things automatically (we set it explicitly on some things) 523 // Set the gravity,, don't disable things automatically (we set it explicitly on some things)
860 524
861 d.WorldSetGravity(world, gravityx, gravityy, gravityz); 525 d.WorldSetGravity(world, gravityx, gravityy, gravityz);
862 d.WorldSetContactSurfaceLayer(world, contactsurfacelayer); 526 d.WorldSetContactSurfaceLayer(world, contactsurfacelayer);
863 527
864 d.WorldSetLinearDamping(world, 256f); 528 d.WorldSetLinearDamping(world, 0.002f);
865 d.WorldSetAngularDamping(world, 256f); 529 d.WorldSetAngularDamping(world, 0.002f);
866 d.WorldSetAngularDampingThreshold(world, 256f); 530 d.WorldSetAngularDampingThreshold(world, 0f);
867 d.WorldSetLinearDampingThreshold(world, 256f); 531 d.WorldSetLinearDampingThreshold(world, 0f);
868 d.WorldSetMaxAngularSpeed(world, 256f); 532 d.WorldSetMaxAngularSpeed(world, 100f);
533
534 d.WorldSetCFM(world,1e-6f); // a bit harder than default
535 //d.WorldSetCFM(world, 1e-4f); // a bit harder than default
536 d.WorldSetERP(world, 0.6f); // higher than original
869 537
870 // Set how many steps we go without running collision testing 538 // Set how many steps we go without running collision testing
871 // This is in addition to the step size. 539 // This is in addition to the step size.
872 // Essentially Steps * m_physicsiterations 540 // Essentially Steps * m_physicsiterations
873 d.WorldSetQuickStepNumIterations(world, m_physicsiterations); 541 d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
874 //d.WorldSetContactMaxCorrectingVel(world, 1000.0f);
875 542
876 for (int i = 0; i < staticPrimspace.GetLength(0); i++) 543 d.WorldSetContactMaxCorrectingVel(world, 60.0f);
877 { 544
878 for (int j = 0; j < staticPrimspace.GetLength(1); j++) 545 spacesPerMeter = 1 / metersInSpace;
546 spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeter);
547 spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeter);
548
549 staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY];
550
551 // create all spaces now
552 int i, j;
553 IntPtr newspace;
554
555 for (i = 0; i < spaceGridMaxX; i++)
556 for (j = 0; j < spaceGridMaxY; j++)
879 { 557 {
880 staticPrimspace[i, j] = IntPtr.Zero; 558 newspace = d.HashSpaceCreate(StaticSpace);
559 d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space);
560 waitForSpaceUnlock(newspace);
561 d.SpaceSetSublevel(newspace, 2);
562 d.HashSpaceSetLevels(newspace, -2, 8);
563 d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space |
564 CollisionCategories.Geom |
565 CollisionCategories.Land |
566 CollisionCategories.Water |
567 CollisionCategories.Phantom |
568 CollisionCategories.VolumeDtc
569 ));
570 d.GeomSetCollideBits(newspace, 0);
571
572 staticPrimspace[i, j] = newspace;
573 }
574 // let this now be real maximum values
575 spaceGridMaxX--;
576 spaceGridMaxY--;
577
578 // create 4 off world spaces (x<0,x>max,y<0,y>max)
579 staticPrimspaceOffRegion = new IntPtr[4];
580
581 for (i = 0; i < 4; i++)
582 {
583 newspace = d.HashSpaceCreate(StaticSpace);
584 d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space);
585 waitForSpaceUnlock(newspace);
586 d.SpaceSetSublevel(newspace, 2);
587 d.HashSpaceSetLevels(newspace, -2, 8);
588 d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space |
589 CollisionCategories.Geom |
590 CollisionCategories.Land |
591 CollisionCategories.Water |
592 CollisionCategories.Phantom |
593 CollisionCategories.VolumeDtc
594 ));
595 d.GeomSetCollideBits(newspace, 0);
596
597 staticPrimspaceOffRegion[i] = newspace;
881 } 598 }
882 }
883 599
884 _worldInitialized = true; 600 m_lastframe = DateTime.UtcNow;
601 m_lastMeshExpire = m_lastframe;
885 } 602 }
886 603
887// internal void waitForSpaceUnlock(IntPtr space) 604 internal void waitForSpaceUnlock(IntPtr space)
888// { 605 {
889// //if (space != IntPtr.Zero) 606 //if (space != IntPtr.Zero)
890// //while (d.SpaceLockQuery(space)) { } // Wait and do nothing 607 //while (d.SpaceLockQuery(space)) { } // Wait and do nothing
891// } 608 }
892
893// /// <summary>
894// /// Debug space message for printing the space that a prim/avatar is in.
895// /// </summary>
896// /// <param name="pos"></param>
897// /// <returns>Returns which split up space the given position is in.</returns>
898// public string whichspaceamIin(Vector3 pos)
899// {
900// return calculateSpaceForGeom(pos).ToString();
901// }
902 609
903 #region Collision Detection 610 #region Collision Detection
904 611
905 /// <summary> 612 // sets a global contact for a joint for contactgeom , and base contact description)
906 /// Collides two geometries. 613
907 /// </summary> 614 private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom, float mu, float bounce, float cfm, float erpscale, float dscale)
908 /// <returns></returns>
909 /// <param name='geom1'></param>
910 /// <param name='geom2'>/param>
911 /// <param name='maxContacts'></param>
912 /// <param name='contactsArray'></param>
913 /// <param name='contactGeomSize'></param>
914 private int CollideGeoms(
915 IntPtr geom1, IntPtr geom2, int maxContacts, Ode.NET.d.ContactGeom[] contactsArray, int contactGeomSize)
916 { 615 {
917 int count; 616 if (GlobalContactsArray == IntPtr.Zero || m_global_contactcount >= maxContactsbeforedeath)
617 return IntPtr.Zero;
918 618
919 lock (OdeScene.UniversalColliderSyncObject) 619 float erp = contactGeom.depth;
920 { 620 erp *= erpscale;
921 // We do this inside the lock so that we don't count any delay in acquiring it 621 if (erp < minERP)
922 if (CollectStats) 622 erp = minERP;
923 m_nativeCollisionStartTick = Util.EnvironmentTickCount(); 623 else if (erp > MaxERP)
624 erp = MaxERP;
924 625
925 count = d.Collide(geom1, geom2, maxContacts, contactsArray, contactGeomSize); 626 float depth = contactGeom.depth * dscale;
926 } 627 if (depth > 0.5f)
628 depth = 0.5f;
629
630 d.Contact newcontact = new d.Contact();
631 newcontact.geom.depth = depth;
632 newcontact.geom.g1 = contactGeom.g1;
633 newcontact.geom.g2 = contactGeom.g2;
634 newcontact.geom.pos = contactGeom.pos;
635 newcontact.geom.normal = contactGeom.normal;
636 newcontact.geom.side1 = contactGeom.side1;
637 newcontact.geom.side2 = contactGeom.side2;
927 638
928 // We do this outside the lock so that any waiting threads aren't held up, though the effect is probably 639 // this needs bounce also
929 // negligable 640 newcontact.surface.mode = comumContactFlags;
930 if (CollectStats) 641 newcontact.surface.mu = mu;
931 m_stats[ODENativeGeomCollisionFrameMsStatName] 642 newcontact.surface.bounce = bounce;
932 += Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick); 643 newcontact.surface.soft_cfm = cfm;
644 newcontact.surface.soft_erp = erp;
933 645
934 return count; 646 IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf));
647 Marshal.StructureToPtr(newcontact, contact, true);
648 return d.JointCreateContactPtr(world, contactgroup, contact);
935 } 649 }
936 650
937 /// <summary> 651 private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom)
938 /// Collide two spaces or a space and a geometry.
939 /// </summary>
940 /// <param name='space1'></param>
941 /// <param name='space2'>/param>
942 /// <param name='data'></param>
943 private void CollideSpaces(IntPtr space1, IntPtr space2, IntPtr data)
944 { 652 {
945 if (CollectStats) 653 if (ContactgeomsArray == IntPtr.Zero || index >= contactsPerCollision)
946 { 654 return false;
947 m_inCollisionTiming = true;
948 m_nativeCollisionStartTick = Util.EnvironmentTickCount();
949 }
950
951 d.SpaceCollide2(space1, space2, data, nearCallback);
952 655
953 if (CollectStats && m_inCollisionTiming) 656 IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf));
954 { 657 newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom));
955 m_stats[ODENativeSpaceCollisionFrameMsStatName] 658 return true;
956 += Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick);
957 m_inCollisionTiming = false;
958 }
959 } 659 }
960 660
961 /// <summary> 661 /// <summary>
@@ -964,76 +664,50 @@ namespace OpenSim.Region.Physics.OdePlugin
964 /// <param name="space">The space that contains the geoms. Remember, spaces are also geoms</param> 664 /// <param name="space">The space that contains the geoms. Remember, spaces are also geoms</param>
965 /// <param name="g1">a geometry or space</param> 665 /// <param name="g1">a geometry or space</param>
966 /// <param name="g2">another geometry or space</param> 666 /// <param name="g2">another geometry or space</param>
667 ///
668
967 private void near(IntPtr space, IntPtr g1, IntPtr g2) 669 private void near(IntPtr space, IntPtr g1, IntPtr g2)
968 { 670 {
969 if (CollectStats && m_inCollisionTiming)
970 {
971 m_stats[ODENativeSpaceCollisionFrameMsStatName]
972 += Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick);
973 m_inCollisionTiming = false;
974 }
975
976// m_log.DebugFormat("[PHYSICS]: Colliding {0} and {1} in {2}", g1, g2, space);
977 // no lock here! It's invoked from within Simulate(), which is thread-locked 671 // no lock here! It's invoked from within Simulate(), which is thread-locked
978 672
673 if (m_global_contactcount >= maxContactsbeforedeath)
674 return;
675
979 // Test if we're colliding a geom with a space. 676 // Test if we're colliding a geom with a space.
980 // If so we have to drill down into the space recursively 677 // If so we have to drill down into the space recursively
981 678
679 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
680 return;
681
982 if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2)) 682 if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
983 { 683 {
984 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
985 return;
986
987 // Separating static prim geometry spaces.
988 // We'll be calling near recursivly if one 684 // We'll be calling near recursivly if one
989 // of them is a space to find all of the 685 // of them is a space to find all of the
990 // contact points in the space 686 // contact points in the space
991 try 687 try
992 { 688 {
993 CollideSpaces(g1, g2, IntPtr.Zero); 689 d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
994 } 690 }
995 catch (AccessViolationException) 691 catch (AccessViolationException)
996 { 692 {
997 m_log.Error("[ODE SCENE]: Unable to collide test a space"); 693 m_log.Warn("[PHYSICS]: Unable to collide test a space");
998 return; 694 return;
999 } 695 }
1000 //Colliding a space or a geom with a space or a geom. so drill down 696 //here one should check collisions of geoms inside a space
1001 697 // but on each space we only should have geoms that not colide amoung each other
1002 //Collide all geoms in each space.. 698 // so we don't dig inside spaces
1003 //if (d.GeomIsSpace(g1)) d.SpaceCollide(g1, IntPtr.Zero, nearCallback);
1004 //if (d.GeomIsSpace(g2)) d.SpaceCollide(g2, IntPtr.Zero, nearCallback);
1005 return; 699 return;
1006 } 700 }
1007 701
1008 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero) 702 // get geom bodies to check if we already a joint contact
1009 return; 703 // guess this shouldn't happen now
1010
1011 IntPtr b1 = d.GeomGetBody(g1); 704 IntPtr b1 = d.GeomGetBody(g1);
1012 IntPtr b2 = d.GeomGetBody(g2); 705 IntPtr b2 = d.GeomGetBody(g2);
1013 706
1014 // d.GeomClassID id = d.GeomGetClass(g1); 707 // d.GeomClassID id = d.GeomGetClass(g1);
1015 708
1016 String name1 = null;
1017 String name2 = null;
1018
1019 if (!geom_name_map.TryGetValue(g1, out name1))
1020 {
1021 name1 = "null";
1022 }
1023 if (!geom_name_map.TryGetValue(g2, out name2))
1024 {
1025 name2 = "null";
1026 }
1027
1028 //if (id == d.GeomClassId.TriMeshClass)
1029 //{
1030 // m_log.InfoFormat("near: A collision was detected between {1} and {2}", 0, name1, name2);
1031 //m_log.Debug("near: A collision was detected between {1} and {2}", 0, name1, name2);
1032 //}
1033
1034 // Figure out how many contact points we have 709 // Figure out how many contact points we have
1035 int count = 0; 710 int count = 0;
1036
1037 try 711 try
1038 { 712 {
1039 // Colliding Geom To Geom 713 // Colliding Geom To Geom
@@ -1045,914 +719,611 @@ namespace OpenSim.Region.Physics.OdePlugin
1045 if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact)) 719 if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
1046 return; 720 return;
1047 721
1048 count = CollideGeoms(g1, g2, contacts.Length, contacts, d.ContactGeom.SizeOf); 722// debug
723 PhysicsActor dp2;
724 if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass)
725 {
726 d.AABB aabb;
727 d.GeomGetAABB(g2, out aabb);
728 float x = aabb.MaxX - aabb.MinX;
729 float y = aabb.MaxY - aabb.MinY;
730 float z = aabb.MaxZ - aabb.MinZ;
731 if (x > 60.0f || y > 60.0f || z > 60.0f)
732 {
733 if (!actor_name_map.TryGetValue(g2, out dp2))
734 m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
735 else
736 m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})",
737 dp2.Name, dp2.Size, x, y, z,
738 dp2.Position.ToString(),
739 dp2.Orientation.ToString(),
740 dp2.Orientation.Length());
741 return;
742 }
743 }
744//
745
1049 746
1050 // All code after this is only relevant if we have any collisions
1051 if (count <= 0)
1052 return;
1053 747
1054 if (count > contacts.Length) 748 if(d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc ||
1055 m_log.Error("[ODE SCENE]: Got " + count + " contacts when we asked for a maximum of " + contacts.Length); 749 d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc)
750 {
751 int cflags;
752 unchecked
753 {
754 cflags = (int)(1 | d.CONTACTS_UNIMPORTANT);
755 }
756 count = d.CollidePtr(g1, g2, cflags, ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
757 }
758 else
759 count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
1056 } 760 }
1057 catch (SEHException) 761 catch (SEHException)
1058 { 762 {
1059 m_log.Error( 763 m_log.Error("[PHYSICS]: The Operating system shut down ODE because of corrupt memory. This could be a result of really irregular terrain. If this repeats continuously, restart using Basic Physics and terrain fill your terrain. Restarting the sim.");
1060 "[ODE SCENE]: The Operating system shut down ODE because of corrupt memory. This could be a result of really irregular terrain. If this repeats continuously, restart using Basic Physics and terrain fill your terrain. Restarting the sim."); 764// ode.drelease(world);
1061 base.TriggerPhysicsBasedRestart(); 765 base.TriggerPhysicsBasedRestart();
1062 } 766 }
1063 catch (Exception e) 767 catch (Exception e)
1064 { 768 {
1065 m_log.ErrorFormat("[ODE SCENE]: Unable to collide test an object: {0}", e.Message); 769 m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
1066 return; 770 return;
1067 } 771 }
1068 772
773 // contacts done
774 if (count == 0)
775 return;
776
777 // try get physical actors
1069 PhysicsActor p1; 778 PhysicsActor p1;
1070 PhysicsActor p2; 779 PhysicsActor p2;
1071 780
1072 p1ExpectedPoints = 0;
1073 p2ExpectedPoints = 0;
1074
1075 if (!actor_name_map.TryGetValue(g1, out p1)) 781 if (!actor_name_map.TryGetValue(g1, out p1))
1076 { 782 {
1077 p1 = PANull; 783 m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 1");
784 return;
1078 } 785 }
1079 786
1080 if (!actor_name_map.TryGetValue(g2, out p2)) 787 if (!actor_name_map.TryGetValue(g2, out p2))
1081 { 788 {
1082 p2 = PANull; 789 m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
790 return;
1083 } 791 }
1084 792
1085 ContactPoint maxDepthContact = new ContactPoint(); 793 // update actors collision score
1086 if (p1.CollisionScore + count >= float.MaxValue) 794 if (p1.CollisionScore >= float.MaxValue - count)
1087 p1.CollisionScore = 0; 795 p1.CollisionScore = 0;
1088 p1.CollisionScore += count; 796 p1.CollisionScore += count;
1089 797
1090 if (p2.CollisionScore + count >= float.MaxValue) 798 if (p2.CollisionScore >= float.MaxValue - count)
1091 p2.CollisionScore = 0; 799 p2.CollisionScore = 0;
1092 p2.CollisionScore += count; 800 p2.CollisionScore += count;
1093 801
1094 for (int i = 0; i < count; i++) 802 // get first contact
1095 { 803 d.ContactGeom curContact = new d.ContactGeom();
1096 d.ContactGeom curContact = contacts[i]; 804 if (!GetCurContactGeom(0, ref curContact))
1097 805 return;
1098 if (curContact.depth > maxDepthContact.PenetrationDepth) 806 // for now it's the one with max depth
1099 { 807 ContactPoint maxDepthContact = new ContactPoint(
1100 maxDepthContact = new ContactPoint(
1101 new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z), 808 new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
1102 new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z), 809 new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
1103 curContact.depth 810 curContact.depth
1104 ); 811 );
1105 } 812 // do volume detection case
1106 813 if (
1107 //m_log.Warn("[CCOUNT]: " + count); 814 (p1.IsVolumeDtc || p2.IsVolumeDtc))
1108 IntPtr joint; 815 {
1109 // If we're colliding with terrain, use 'TerrainContact' instead of contact. 816 collision_accounting_events(p1, p2, maxDepthContact);
1110 // allows us to have different settings 817 return;
1111 818 }
1112 // We only need to test p2 for 'jump crouch purposes'
1113 if (p2 is OdeCharacter && p1.PhysicsActorType == (int)ActorTypes.Prim)
1114 {
1115 // Testing if the collision is at the feet of the avatar
1116 819
1117 //m_log.DebugFormat("[PHYSICS]: {0} - {1} - {2} - {3}", curContact.pos.Z, p2.Position.Z, (p2.Position.Z - curContact.pos.Z), (p2.Size.Z * 0.6f)); 820 // big messy collision analises
1118 if ((p2.Position.Z - curContact.pos.Z) > (p2.Size.Z * 0.6f))
1119 p2.IsColliding = true;
1120 }
1121 else
1122 {
1123 p2.IsColliding = true;
1124 }
1125
1126 //if ((framecount % m_returncollisions) == 0)
1127 821
1128 switch (p1.PhysicsActorType) 822 Vector3 normoverride = Vector3.Zero; //damm c#
1129 {
1130 case (int)ActorTypes.Agent:
1131 p1ExpectedPoints = avatarExpectedContacts;
1132 p2.CollidingObj = true;
1133 break;
1134 case (int)ActorTypes.Prim:
1135 if (p1 != null && p1 is OdePrim)
1136 p1ExpectedPoints = ((OdePrim) p1).ExpectedCollisionContacts;
1137 823
1138 if (p2.Velocity.LengthSquared() > 0.0f) 824 float mu = 0;
1139 p2.CollidingObj = true; 825 float bounce = 0;
1140 break; 826 float cfm = 0.0001f;
1141 case (int)ActorTypes.Unknown: 827 float erpscale = 1.0f;
1142 p2.CollidingGround = true; 828 float dscale = 1.0f;
1143 break; 829 bool IgnoreNegSides = false;
1144 default:
1145 p2.CollidingGround = true;
1146 break;
1147 }
1148 830
1149 // we don't want prim or avatar to explode 831 ContactData contactdata1 = new ContactData(0, 0, false);
832 ContactData contactdata2 = new ContactData(0, 0, false);
1150 833
1151 #region InterPenetration Handling - Unintended physics explosions 834 bool dop1foot = false;
1152# region disabled code1 835 bool dop2foot = false;
836 bool ignore = false;
837 bool AvanormOverride = false;
1153 838
1154 if (curContact.depth >= 0.08f) 839 switch (p1.PhysicsActorType)
1155 { 840 {
1156 //This is disabled at the moment only because it needs more tweaking 841 case (int)ActorTypes.Agent:
1157 //It will eventually be uncommented
1158 /*
1159 if (contact.depth >= 1.00f)
1160 { 842 {
1161 //m_log.Debug("[PHYSICS]: " + contact.depth.ToString()); 843 AvanormOverride = true;
1162 } 844 Vector3 tmp = p2.Position - p1.Position;
845 normoverride = p2.Velocity - p1.Velocity;
846 mu = normoverride.LengthSquared();
1163 847
1164 //If you interpenetrate a prim with an agent 848 if (mu > 1e-6)
1165 if ((p2.PhysicsActorType == (int) ActorTypes.Agent &&
1166 p1.PhysicsActorType == (int) ActorTypes.Prim) ||
1167 (p1.PhysicsActorType == (int) ActorTypes.Agent &&
1168 p2.PhysicsActorType == (int) ActorTypes.Prim))
1169 {
1170
1171 //contact.depth = contact.depth * 4.15f;
1172 /*
1173 if (p2.PhysicsActorType == (int) ActorTypes.Agent)
1174 { 849 {
1175 p2.CollidingObj = true; 850 mu = 1.0f / (float)Math.Sqrt(mu);
1176 contact.depth = 0.003f; 851 normoverride *= mu;
1177 p2.Velocity = p2.Velocity + new PhysicsVector(0, 0, 2.5f); 852 mu = Vector3.Dot(tmp, normoverride);
1178 OdeCharacter character = (OdeCharacter) p2; 853 if (mu > 0)
1179 character.SetPidStatus(true); 854 normoverride *= -1;
1180 contact.pos = new d.Vector3(contact.pos.X + (p1.Size.X / 2), contact.pos.Y + (p1.Size.Y / 2), contact.pos.Z + (p1.Size.Z / 2));
1181
1182 } 855 }
1183 else 856 else
1184 { 857 {
1185 858 tmp.Normalize();
1186 //contact.depth = 0.0000000f; 859 normoverride = -tmp;
1187 } 860 }
1188 if (p1.PhysicsActorType == (int) ActorTypes.Agent)
1189 {
1190 861
1191 p1.CollidingObj = true; 862 switch (p2.PhysicsActorType)
1192 contact.depth = 0.003f;
1193 p1.Velocity = p1.Velocity + new PhysicsVector(0, 0, 2.5f);
1194 contact.pos = new d.Vector3(contact.pos.X + (p2.Size.X / 2), contact.pos.Y + (p2.Size.Y / 2), contact.pos.Z + (p2.Size.Z / 2));
1195 OdeCharacter character = (OdeCharacter)p1;
1196 character.SetPidStatus(true);
1197 }
1198 else
1199 { 863 {
864 case (int)ActorTypes.Agent:
865 p1.CollidingObj = true;
866 p2.CollidingObj = true;
867 break;
868
869 case (int)ActorTypes.Prim:
870 if (p2.Velocity.LengthSquared() > 0.0f)
871 p2.CollidingObj = true;
872 dop1foot = true;
873 break;
1200 874
1201 //contact.depth = 0.0000000f; 875 default:
876 ignore = true; // avatar to terrain and water ignored
877 break;
1202 } 878 }
1203 879 break;
1204
1205
1206 }
1207*/
1208 // If you interpenetrate a prim with another prim
1209 /*
1210 if (p1.PhysicsActorType == (int) ActorTypes.Prim && p2.PhysicsActorType == (int) ActorTypes.Prim)
1211 {
1212 #region disabledcode2
1213 //OdePrim op1 = (OdePrim)p1;
1214 //OdePrim op2 = (OdePrim)p2;
1215 //op1.m_collisionscore++;
1216 //op2.m_collisionscore++;
1217
1218 //if (op1.m_collisionscore > 8000 || op2.m_collisionscore > 8000)
1219 //{
1220 //op1.m_taintdisable = true;
1221 //AddPhysicsActorTaint(p1);
1222 //op2.m_taintdisable = true;
1223 //AddPhysicsActorTaint(p2);
1224 //}
1225
1226 //if (contact.depth >= 0.25f)
1227 //{
1228 // Don't collide, one or both prim will expld.
1229
1230 //op1.m_interpenetrationcount++;
1231 //op2.m_interpenetrationcount++;
1232 //interpenetrations_before_disable = 200;
1233 //if (op1.m_interpenetrationcount >= interpenetrations_before_disable)
1234 //{
1235 //op1.m_taintdisable = true;
1236 //AddPhysicsActorTaint(p1);
1237 //}
1238 //if (op2.m_interpenetrationcount >= interpenetrations_before_disable)
1239 //{
1240 // op2.m_taintdisable = true;
1241 //AddPhysicsActorTaint(p2);
1242 //}
1243
1244 //contact.depth = contact.depth / 8f;
1245 //contact.normal = new d.Vector3(0, 0, 1);
1246 //}
1247 //if (op1.m_disabled || op2.m_disabled)
1248 //{
1249 //Manually disabled objects stay disabled
1250 //contact.depth = 0f;
1251 //}
1252 #endregion
1253 } 880 }
1254 */ 881
1255#endregion 882 case (int)ActorTypes.Prim:
1256 if (curContact.depth >= 1.00f) 883 switch (p2.PhysicsActorType)
1257 { 884 {
1258 //m_log.Info("[P]: " + contact.depth.ToString()); 885 case (int)ActorTypes.Agent:
1259 if ((p2.PhysicsActorType == (int) ActorTypes.Agent && 886 AvanormOverride = true;
1260 p1.PhysicsActorType == (int) ActorTypes.Unknown) || 887
1261 (p1.PhysicsActorType == (int) ActorTypes.Agent && 888 Vector3 tmp = p2.Position - p1.Position;
1262 p2.PhysicsActorType == (int) ActorTypes.Unknown)) 889 normoverride = p2.Velocity - p1.Velocity;
1263 { 890 mu = normoverride.LengthSquared();
1264 if (p2.PhysicsActorType == (int) ActorTypes.Agent) 891 if (mu > 1e-6)
1265 { 892 {
1266 if (p2 is OdeCharacter) 893 mu = 1.0f / (float)Math.Sqrt(mu);
1267 { 894 normoverride *= mu;
1268 OdeCharacter character = (OdeCharacter) p2; 895 mu = Vector3.Dot(tmp, normoverride);
1269 896 if (mu > 0)
1270 //p2.CollidingObj = true; 897 normoverride *= -1;
1271 curContact.depth = 0.00000003f; 898 }
1272 p2.Velocity = p2.Velocity + new Vector3(0f, 0f, 0.5f); 899 else
1273 curContact.pos = 900 {
1274 new d.Vector3(curContact.pos.X + (p1.Size.X/2), 901 tmp.Normalize();
1275 curContact.pos.Y + (p1.Size.Y/2), 902 normoverride = -tmp;
1276 curContact.pos.Z + (p1.Size.Z/2)); 903 }
1277 character.SetPidStatus(true); 904
1278 } 905 bounce = 0;
906 mu = 0;
907 cfm = 0.0001f;
908
909 dop2foot = true;
910 if (p1.Velocity.LengthSquared() > 0.0f)
911 p1.CollidingObj = true;
912 break;
913
914 case (int)ActorTypes.Prim:
915 if ((p1.Velocity - p2.Velocity).LengthSquared() > 0.0f)
916 {
917 p1.CollidingObj = true;
918 p2.CollidingObj = true;
1279 } 919 }
920 p1.getContactData(ref contactdata1);
921 p2.getContactData(ref contactdata2);
922 bounce = contactdata1.bounce * contactdata2.bounce;
923 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
924
925 cfm = p1.Mass;
926 if (cfm > p2.Mass)
927 cfm = p2.Mass;
928 dscale = 10 / cfm;
929 dscale = (float)Math.Sqrt(dscale);
930 if (dscale > 1.0f)
931 dscale = 1.0f;
932 erpscale = cfm * 0.01f;
933 cfm = 0.0001f / cfm;
934 if (cfm > 0.01f)
935 cfm = 0.01f;
936 else if (cfm < 0.00001f)
937 cfm = 0.00001f;
938
939 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
940 mu *= frictionMovementMult;
1280 941
1281 if (p1.PhysicsActorType == (int) ActorTypes.Agent) 942 break;
943
944 case (int)ActorTypes.Ground:
945 p1.getContactData(ref contactdata1);
946 bounce = contactdata1.bounce * TerrainBounce;
947 mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction);
948 if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f)
949 mu *= frictionMovementMult;
950 p1.CollidingGround = true;
951
952 cfm = p1.Mass;
953 dscale = 10 / cfm;
954 dscale = (float)Math.Sqrt(dscale);
955 if (dscale > 1.0f)
956 dscale = 1.0f;
957 erpscale = cfm * 0.01f;
958 cfm = 0.0001f / cfm;
959 if (cfm > 0.01f)
960 cfm = 0.01f;
961 else if (cfm < 0.00001f)
962 cfm = 0.00001f;
963
964 if (d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass)
1282 { 965 {
1283 if (p1 is OdeCharacter) 966 if (curContact.side1 > 0)
1284 { 967 IgnoreNegSides = true;
1285 OdeCharacter character = (OdeCharacter) p1;
1286
1287 //p2.CollidingObj = true;
1288 curContact.depth = 0.00000003f;
1289 p1.Velocity = p1.Velocity + new Vector3(0f, 0f, 0.5f);
1290 curContact.pos =
1291 new d.Vector3(curContact.pos.X + (p1.Size.X/2),
1292 curContact.pos.Y + (p1.Size.Y/2),
1293 curContact.pos.Z + (p1.Size.Z/2));
1294 character.SetPidStatus(true);
1295 }
1296 } 968 }
1297 } 969 break;
970
971 case (int)ActorTypes.Water:
972 default:
973 ignore = true;
974 break;
1298 } 975 }
1299 } 976 break;
977
978 case (int)ActorTypes.Ground:
979 if (p2.PhysicsActorType == (int)ActorTypes.Prim)
980 {
981 p2.CollidingGround = true;
982 p2.getContactData(ref contactdata2);
983 bounce = contactdata2.bounce * TerrainBounce;
984 mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction);
985
986 cfm = p2.Mass;
987 dscale = 10 / cfm;
988 dscale = (float)Math.Sqrt(dscale);
989
990 if (dscale > 1.0f)
991 dscale = 1.0f;
1300 992
1301 #endregion 993 erpscale = cfm * 0.01f;
994 cfm = 0.0001f / cfm;
995 if (cfm > 0.01f)
996 cfm = 0.01f;
997 else if (cfm < 0.00001f)
998 cfm = 0.00001f;
1302 999
1303 // Logic for collision handling 1000 if (curContact.side1 > 0) // should be 2 ?
1304 // Note, that if *all* contacts are skipped (VolumeDetect) 1001 IgnoreNegSides = true;
1305 // The prim still detects (and forwards) collision events but
1306 // appears to be phantom for the world
1307 Boolean skipThisContact = false;
1308 1002
1309 if ((p1 is OdePrim) && (((OdePrim)p1).m_isVolumeDetect)) 1003 if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f)
1310 skipThisContact = true; // No collision on volume detect prims 1004 mu *= frictionMovementMult;
1005 }
1006 else
1007 ignore = true;
1008 break;
1009
1010 case (int)ActorTypes.Water:
1011 default:
1012 break;
1013 }
1014 if (ignore)
1015 return;
1311 1016
1312 if (!skipThisContact && (p2 is OdePrim) && (((OdePrim)p2).m_isVolumeDetect)) 1017 IntPtr Joint;
1313 skipThisContact = true; // No collision on volume detect prims
1314 1018
1315 if (!skipThisContact && curContact.depth < 0f) 1019 int i = 0;
1316 skipThisContact = true; 1020 while(true)
1021 {
1317 1022
1318 if (!skipThisContact && checkDupe(curContact, p2.PhysicsActorType)) 1023 if (IgnoreNegSides && curContact.side1 < 0)
1319 skipThisContact = true; 1024 {
1025 if (++i >= count)
1026 break;
1320 1027
1321 const int maxContactsbeforedeath = 4000; 1028 if (!GetCurContactGeom(i, ref curContact))
1322 joint = IntPtr.Zero; 1029 break;
1030 }
1031 else
1323 1032
1324 if (!skipThisContact)
1325 { 1033 {
1326 _perloopContact.Add(curContact);
1327 1034
1328 if (name1 == "Terrain" || name2 == "Terrain") 1035 if (AvanormOverride)
1329 { 1036 {
1330 if ((p2.PhysicsActorType == (int) ActorTypes.Agent) && 1037 if (curContact.depth > 0.3f)
1331 (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f))
1332 { 1038 {
1333 p2ExpectedPoints = avatarExpectedContacts; 1039 if (dop1foot && (p1.Position.Z - curContact.pos.Z) > (p1.Size.Z - avCapRadius) * 0.5f)
1334 // Avatar is moving on terrain, use the movement terrain contact 1040 p1.IsColliding = true;
1335 AvatarMovementTerrainContact.geom = curContact; 1041 if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f)
1336 1042 p2.IsColliding = true;
1337 if (m_global_contactcount < maxContactsbeforedeath) 1043 curContact.normal.X = normoverride.X;
1338 { 1044 curContact.normal.Y = normoverride.Y;
1339 joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementTerrainContact); 1045 curContact.normal.Z = normoverride.Z;
1340 m_global_contactcount++;
1341 }
1342 } 1046 }
1047
1343 else 1048 else
1344 { 1049 {
1345 if (p2.PhysicsActorType == (int)ActorTypes.Agent) 1050 if (dop1foot)
1346 {
1347 p2ExpectedPoints = avatarExpectedContacts;
1348 // Avatar is standing on terrain, use the non moving terrain contact
1349 TerrainContact.geom = curContact;
1350
1351 if (m_global_contactcount < maxContactsbeforedeath)
1352 {
1353 joint = d.JointCreateContact(world, contactgroup, ref TerrainContact);
1354 m_global_contactcount++;
1355 }
1356 }
1357 else
1358 { 1051 {
1359 if (p2.PhysicsActorType == (int)ActorTypes.Prim && p1.PhysicsActorType == (int)ActorTypes.Prim) 1052 float sz = p1.Size.Z;
1053 Vector3 vtmp = p1.Position;
1054 float ppos = curContact.pos.Z - vtmp.Z + (sz - avCapRadius) * 0.5f;
1055 if (ppos > 0f)
1360 { 1056 {
1361 // prim prim contact 1057 if (!p1.Flying)
1362 // int pj294950 = 0;
1363 int movintYN = 0;
1364 int material = (int) Material.Wood;
1365 // prim terrain contact
1366 if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)
1367 {
1368 movintYN = 1;
1369 }
1370
1371 if (p2 is OdePrim)
1372 { 1058 {
1373 material = ((OdePrim) p2).m_material; 1059 d.AABB aabb;
1374 p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts; 1060 d.GeomGetAABB(g2, out aabb);
1375 } 1061 float tmp = vtmp.Z - sz * .18f;
1376
1377 // Unnessesary because p1 is defined above
1378 //if (p1 is OdePrim)
1379 // {
1380 // p1ExpectedPoints = ((OdePrim)p1).ExpectedCollisionContacts;
1381 // }
1382 //m_log.DebugFormat("Material: {0}", material);
1383
1384 m_materialContacts[material, movintYN].geom = curContact;
1385 1062
1386 if (m_global_contactcount < maxContactsbeforedeath) 1063 if (aabb.MaxZ < tmp)
1387 { 1064 {
1388 joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]); 1065 vtmp.X = curContact.pos.X - vtmp.X;
1389 m_global_contactcount++; 1066 vtmp.Y = curContact.pos.Y - vtmp.Y;
1067 vtmp.Z = -0.2f;
1068 vtmp.Normalize();
1069 curContact.normal.X = vtmp.X;
1070 curContact.normal.Y = vtmp.Y;
1071 curContact.normal.Z = vtmp.Z;
1072 }
1390 } 1073 }
1391 } 1074 }
1392 else 1075 else
1393 { 1076 p1.IsColliding = true;
1394 int movintYN = 0;
1395 // prim terrain contact
1396 if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)
1397 {
1398 movintYN = 1;
1399 }
1400 1077
1401 int material = (int)Material.Wood;
1402
1403 if (p2 is OdePrim)
1404 {
1405 material = ((OdePrim)p2).m_material;
1406 p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts;
1407 }
1408
1409 //m_log.DebugFormat("Material: {0}", material);
1410 m_materialContacts[material, movintYN].geom = curContact;
1411
1412 if (m_global_contactcount < maxContactsbeforedeath)
1413 {
1414 joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]);
1415 m_global_contactcount++;
1416 }
1417 }
1418 } 1078 }
1419 }
1420 //if (p2.PhysicsActorType == (int)ActorTypes.Prim)
1421 //{
1422 //m_log.Debug("[PHYSICS]: prim contacting with ground");
1423 //}
1424 }
1425 else if (name1 == "Water" || name2 == "Water")
1426 {
1427 /*
1428 if ((p2.PhysicsActorType == (int) ActorTypes.Prim))
1429 {
1430 }
1431 else
1432 {
1433 }
1434 */
1435 //WaterContact.surface.soft_cfm = 0.0000f;
1436 //WaterContact.surface.soft_erp = 0.00000f;
1437 if (curContact.depth > 0.1f)
1438 {
1439 curContact.depth *= 52;
1440 //contact.normal = new d.Vector3(0, 0, 1);
1441 //contact.pos = new d.Vector3(0, 0, contact.pos.Z - 5f);
1442 }
1443
1444 WaterContact.geom = curContact;
1445 1079
1446 if (m_global_contactcount < maxContactsbeforedeath) 1080 if (dop2foot)
1447 {
1448 joint = d.JointCreateContact(world, contactgroup, ref WaterContact);
1449 m_global_contactcount++;
1450 }
1451 //m_log.Info("[PHYSICS]: Prim Water Contact" + contact.depth);
1452 }
1453 else
1454 {
1455 if ((p2.PhysicsActorType == (int)ActorTypes.Agent))
1456 {
1457 p2ExpectedPoints = avatarExpectedContacts;
1458 if ((Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f))
1459 { 1081 {
1460 // Avatar is moving on a prim, use the Movement prim contact 1082 float sz = p2.Size.Z;
1461 AvatarMovementprimContact.geom = curContact; 1083 Vector3 vtmp = p2.Position;
1462 1084 float ppos = curContact.pos.Z - vtmp.Z + (sz - avCapRadius) * 0.5f;
1463 if (m_global_contactcount < maxContactsbeforedeath) 1085 if (ppos > 0f)
1464 { 1086 {
1465 joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementprimContact); 1087 if (!p2.Flying)
1466 m_global_contactcount++; 1088 {
1467 } 1089 d.AABB aabb;
1468 } 1090 d.GeomGetAABB(g1, out aabb);
1469 else 1091 float tmp = vtmp.Z - sz * .18f;
1470 {
1471 // Avatar is standing still on a prim, use the non movement contact
1472 contact.geom = curContact;
1473 1092
1474 if (m_global_contactcount < maxContactsbeforedeath) 1093 if (aabb.MaxZ < tmp)
1475 { 1094 {
1476 joint = d.JointCreateContact(world, contactgroup, ref contact); 1095 vtmp.X = curContact.pos.X - vtmp.X;
1477 m_global_contactcount++; 1096 vtmp.Y = curContact.pos.Y - vtmp.Y;
1097 vtmp.Z = -0.2f;
1098 vtmp.Normalize();
1099 curContact.normal.X = vtmp.X;
1100 curContact.normal.Y = vtmp.Y;
1101 curContact.normal.Z = vtmp.Z;
1102 }
1103 }
1478 } 1104 }
1479 } 1105 else
1480 } 1106 p2.IsColliding = true;
1481 else if (p2.PhysicsActorType == (int)ActorTypes.Prim)
1482 {
1483 //p1.PhysicsActorType
1484 int material = (int)Material.Wood;
1485
1486 if (p2 is OdePrim)
1487 {
1488 material = ((OdePrim)p2).m_material;
1489 p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts;
1490 }
1491
1492 //m_log.DebugFormat("Material: {0}", material);
1493 m_materialContacts[material, 0].geom = curContact;
1494 1107
1495 if (m_global_contactcount < maxContactsbeforedeath)
1496 {
1497 joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, 0]);
1498 m_global_contactcount++;
1499 } 1108 }
1500 } 1109 }
1501 } 1110 }
1502 1111
1503 if (m_global_contactcount < maxContactsbeforedeath && joint != IntPtr.Zero) // stack collide! 1112 Joint = CreateContacJoint(ref curContact, mu, bounce, cfm, erpscale, dscale);
1504 { 1113 d.JointAttach(Joint, b1, b2);
1505 d.JointAttach(joint, b1, b2);
1506 m_global_contactcount++;
1507 }
1508 }
1509
1510 collision_accounting_events(p1, p2, maxDepthContact);
1511
1512 if (count > ((p1ExpectedPoints + p2ExpectedPoints) * 0.25) + (geomContactPointsStartthrottle))
1513 {
1514 // If there are more then 3 contact points, it's likely
1515 // that we've got a pile of objects, so ...
1516 // We don't want to send out hundreds of terse updates over and over again
1517 // so lets throttle them and send them again after it's somewhat sorted out.
1518 p2.ThrottleUpdates = true;
1519 }
1520 //m_log.Debug(count.ToString());
1521 //m_log.Debug("near: A collision was detected between {1} and {2}", 0, name1, name2);
1522 }
1523 }
1524 1114
1525 private bool checkDupe(d.ContactGeom contactGeom, int atype) 1115 if (++m_global_contactcount >= maxContactsbeforedeath)
1526 { 1116 break;
1527 if (!m_filterCollisions)
1528 return false;
1529
1530 bool result = false;
1531 1117
1532 ActorTypes at = (ActorTypes)atype; 1118 if (++i >= count)
1119 break;
1533 1120
1534 foreach (d.ContactGeom contact in _perloopContact) 1121 if (!GetCurContactGeom(i, ref curContact))
1535 { 1122 break;
1536 //if ((contact.g1 == contactGeom.g1 && contact.g2 == contactGeom.g2))
1537 //{
1538 // || (contact.g2 == contactGeom.g1 && contact.g1 == contactGeom.g2)
1539 if (at == ActorTypes.Agent)
1540 {
1541 if (((Math.Abs(contactGeom.normal.X - contact.normal.X) < 1.026f)
1542 && (Math.Abs(contactGeom.normal.Y - contact.normal.Y) < 0.303f)
1543 && (Math.Abs(contactGeom.normal.Z - contact.normal.Z) < 0.065f)))
1544 {
1545 if (Math.Abs(contact.depth - contactGeom.depth) < 0.052f)
1546 {
1547 //contactGeom.depth *= .00005f;
1548 //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
1549 // m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z));
1550 result = true;
1551 break;
1552 }
1553// else
1554// {
1555// //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
1556// }
1557 }
1558// else
1559// {
1560// //m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z));
1561// //int i = 0;
1562// }
1563 }
1564 else if (at == ActorTypes.Prim)
1565 {
1566 //d.AABB aabb1 = new d.AABB();
1567 //d.AABB aabb2 = new d.AABB();
1568 1123
1569 //d.GeomGetAABB(contactGeom.g2, out aabb2); 1124 if (curContact.depth > maxDepthContact.PenetrationDepth)
1570 //d.GeomGetAABB(contactGeom.g1, out aabb1);
1571 //aabb1.
1572 if (((Math.Abs(contactGeom.normal.X - contact.normal.X) < 1.026f) && (Math.Abs(contactGeom.normal.Y - contact.normal.Y) < 0.303f) && (Math.Abs(contactGeom.normal.Z - contact.normal.Z) < 0.065f)))
1573 { 1125 {
1574 if (contactGeom.normal.X == contact.normal.X && contactGeom.normal.Y == contact.normal.Y && contactGeom.normal.Z == contact.normal.Z) 1126 maxDepthContact.Position.X = curContact.pos.X;
1575 { 1127 maxDepthContact.Position.Y = curContact.pos.Y;
1576 if (Math.Abs(contact.depth - contactGeom.depth) < 0.272f) 1128 maxDepthContact.Position.Z = curContact.pos.Z;
1577 { 1129 maxDepthContact.SurfaceNormal.X = curContact.normal.X;
1578 result = true; 1130 maxDepthContact.SurfaceNormal.Y = curContact.normal.Y;
1579 break; 1131 maxDepthContact.SurfaceNormal.Z = curContact.normal.Z;
1580 } 1132 maxDepthContact.PenetrationDepth = curContact.depth;
1581 }
1582 //m_log.DebugFormat("[Collision]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
1583 //m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z));
1584 } 1133 }
1585 } 1134 }
1586 } 1135 }
1587 1136
1588 return result; 1137 collision_accounting_events(p1, p2, maxDepthContact);
1138
1139/*
1140 if (notskipedcount > geomContactPointsStartthrottle)
1141 {
1142 // If there are more then 3 contact points, it's likely
1143 // that we've got a pile of objects, so ...
1144 // We don't want to send out hundreds of terse updates over and over again
1145 // so lets throttle them and send them again after it's somewhat sorted out.
1146 this needs checking so out for now
1147 if (b1 != IntPtr.Zero)
1148 p1.ThrottleUpdates = true;
1149 if (b2 != IntPtr.Zero)
1150 p2.ThrottleUpdates = true;
1151
1152 }
1153 */
1589 } 1154 }
1590 1155
1591 private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact) 1156 private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
1592 { 1157 {
1593 // obj1LocalID = 0; 1158 uint obj2LocalID = 0;
1594 //returncollisions = false;
1595 obj2LocalID = 0;
1596 //ctype = 0;
1597 //cStartStop = 0;
1598 if (!p2.SubscribedEvents() && !p1.SubscribedEvents())
1599 return;
1600 1159
1601 switch ((ActorTypes)p2.PhysicsActorType) 1160 bool p1events = p1.SubscribedEvents();
1602 { 1161 bool p2events = p2.SubscribedEvents();
1603 case ActorTypes.Agent:
1604 cc2 = (OdeCharacter)p2;
1605 1162
1606 // obj1LocalID = cc2.m_localID; 1163 if (p1.IsVolumeDtc)
1607 switch ((ActorTypes)p1.PhysicsActorType) 1164 p2events = false;
1608 { 1165 if (p2.IsVolumeDtc)
1609 case ActorTypes.Agent: 1166 p1events = false;
1610 cc1 = (OdeCharacter)p1;
1611 obj2LocalID = cc1.LocalID;
1612 cc1.AddCollisionEvent(cc2.LocalID, contact);
1613 //ctype = (int)CollisionCategories.Character;
1614
1615 //if (cc1.CollidingObj)
1616 //cStartStop = (int)StatusIndicators.Generic;
1617 //else
1618 //cStartStop = (int)StatusIndicators.Start;
1619
1620 //returncollisions = true;
1621 break;
1622 1167
1623 case ActorTypes.Prim: 1168 if (!p2events && !p1events)
1624 if (p1 is OdePrim) 1169 return;
1625 {
1626 cp1 = (OdePrim) p1;
1627 obj2LocalID = cp1.LocalID;
1628 cp1.AddCollisionEvent(cc2.LocalID, contact);
1629 }
1630 //ctype = (int)CollisionCategories.Geom;
1631 1170
1632 //if (cp1.CollidingObj) 1171 Vector3 vel = Vector3.Zero;
1633 //cStartStop = (int)StatusIndicators.Generic; 1172 if (p2 != null && p2.IsPhysical)
1634 //else 1173 vel = p2.Velocity;
1635 //cStartStop = (int)StatusIndicators.Start;
1636 1174
1637 //returncollisions = true; 1175 if (p1 != null && p1.IsPhysical)
1638 break; 1176 vel -= p1.Velocity;
1639 1177
1640 case ActorTypes.Ground: 1178 contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal);
1641 case ActorTypes.Unknown:
1642 obj2LocalID = 0;
1643 //ctype = (int)CollisionCategories.Land;
1644 //returncollisions = true;
1645 break;
1646 }
1647
1648 cc2.AddCollisionEvent(obj2LocalID, contact);
1649 break;
1650 1179
1180 switch ((ActorTypes)p1.PhysicsActorType)
1181 {
1182 case ActorTypes.Agent:
1651 case ActorTypes.Prim: 1183 case ActorTypes.Prim:
1652
1653 if (p2 is OdePrim)
1654 { 1184 {
1655 cp2 = (OdePrim) p2; 1185 switch ((ActorTypes)p2.PhysicsActorType)
1656
1657 // obj1LocalID = cp2.m_localID;
1658 switch ((ActorTypes) p1.PhysicsActorType)
1659 { 1186 {
1660 case ActorTypes.Agent: 1187 case ActorTypes.Agent:
1661 if (p1 is OdeCharacter)
1662 {
1663 cc1 = (OdeCharacter) p1;
1664 obj2LocalID = cc1.LocalID;
1665 cc1.AddCollisionEvent(cp2.LocalID, contact);
1666 //ctype = (int)CollisionCategories.Character;
1667
1668 //if (cc1.CollidingObj)
1669 //cStartStop = (int)StatusIndicators.Generic;
1670 //else
1671 //cStartStop = (int)StatusIndicators.Start;
1672 //returncollisions = true;
1673 }
1674 break;
1675 case ActorTypes.Prim: 1188 case ActorTypes.Prim:
1676 1189 if (p2events)
1677 if (p1 is OdePrim)
1678 { 1190 {
1679 cp1 = (OdePrim) p1; 1191 AddCollisionEventReporting(p2);
1680 obj2LocalID = cp1.LocalID; 1192 p2.AddCollisionEvent(p1.ParentActor.LocalID, contact);
1681 cp1.AddCollisionEvent(cp2.LocalID, contact);
1682 //ctype = (int)CollisionCategories.Geom;
1683
1684 //if (cp1.CollidingObj)
1685 //cStartStop = (int)StatusIndicators.Generic;
1686 //else
1687 //cStartStop = (int)StatusIndicators.Start;
1688
1689 //returncollisions = true;
1690 } 1193 }
1194 obj2LocalID = p2.ParentActor.LocalID;
1691 break; 1195 break;
1692 1196
1693 case ActorTypes.Ground: 1197 case ActorTypes.Ground:
1694 case ActorTypes.Unknown: 1198 case ActorTypes.Unknown:
1199 default:
1695 obj2LocalID = 0; 1200 obj2LocalID = 0;
1696 //ctype = (int)CollisionCategories.Land;
1697
1698 //returncollisions = true;
1699 break; 1201 break;
1700 } 1202 }
1701 1203 if (p1events)
1702 cp2.AddCollisionEvent(obj2LocalID, contact);
1703 }
1704 break;
1705 }
1706 //if (returncollisions)
1707 //{
1708
1709 //lock (m_storedCollisions)
1710 //{
1711 //cDictKey = obj1LocalID.ToString() + obj2LocalID.ToString() + cStartStop.ToString() + ctype.ToString();
1712 //if (m_storedCollisions.ContainsKey(cDictKey))
1713 //{
1714 //sCollisionData objd = m_storedCollisions[cDictKey];
1715 //objd.NumberOfCollisions += 1;
1716 //objd.lastframe = framecount;
1717 //m_storedCollisions[cDictKey] = objd;
1718 //}
1719 //else
1720 //{
1721 //sCollisionData objd = new sCollisionData();
1722 //objd.ColliderLocalId = obj1LocalID;
1723 //objd.CollidedWithLocalId = obj2LocalID;
1724 //objd.CollisionType = ctype;
1725 //objd.NumberOfCollisions = 1;
1726 //objd.lastframe = framecount;
1727 //objd.StatusIndicator = cStartStop;
1728 //m_storedCollisions.Add(cDictKey, objd);
1729 //}
1730 //}
1731 // }
1732 }
1733
1734 private int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount)
1735 {
1736 /* String name1 = null;
1737 String name2 = null;
1738
1739 if (!geom_name_map.TryGetValue(trimesh, out name1))
1740 { 1204 {
1741 name1 = "null"; 1205 contact.SurfaceNormal = -contact.SurfaceNormal;
1206 AddCollisionEventReporting(p1);
1207 p1.AddCollisionEvent(obj2LocalID, contact);
1742 } 1208 }
1743 if (!geom_name_map.TryGetValue(refObject, out name2)) 1209 break;
1210 }
1211 case ActorTypes.Ground:
1212 case ActorTypes.Unknown:
1213 default:
1214 {
1215 if (p2events && !p2.IsVolumeDtc)
1744 { 1216 {
1745 name2 = "null"; 1217 AddCollisionEventReporting(p2);
1218 p2.AddCollisionEvent(0, contact);
1746 } 1219 }
1747 1220 break;
1748 m_log.InfoFormat("TriArrayCallback: A collision was detected between {1} and {2}", 0, name1, name2); 1221 }
1749 */ 1222 }
1750 return 1;
1751 }
1752
1753 private int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex)
1754 {
1755// String name1 = null;
1756// String name2 = null;
1757//
1758// if (!geom_name_map.TryGetValue(trimesh, out name1))
1759// {
1760// name1 = "null";
1761// }
1762//
1763// if (!geom_name_map.TryGetValue(refObject, out name2))
1764// {
1765// name2 = "null";
1766// }
1767
1768 // m_log.InfoFormat("TriCallback: A collision was detected between {1} and {2}. Index was {3}", 0, name1, name2, triangleIndex);
1769
1770 d.Vector3 v0 = new d.Vector3();
1771 d.Vector3 v1 = new d.Vector3();
1772 d.Vector3 v2 = new d.Vector3();
1773
1774 d.GeomTriMeshGetTriangle(trimesh, 0, ref v0, ref v1, ref v2);
1775 // m_log.DebugFormat("Triangle {0} is <{1},{2},{3}>, <{4},{5},{6}>, <{7},{8},{9}>", triangleIndex, v0.X, v0.Y, v0.Z, v1.X, v1.Y, v1.Z, v2.X, v2.Y, v2.Z);
1776
1777 return 1;
1778 } 1223 }
1779 1224
1780 /// <summary> 1225 /// <summary>
1781 /// This is our collision testing routine in ODE 1226 /// This is our collision testing routine in ODE
1782 /// </summary> 1227 /// </summary>
1228 /// <param name="timeStep"></param>
1783 private void collision_optimized() 1229 private void collision_optimized()
1784 { 1230 {
1785 _perloopContact.Clear(); 1231 lock (_characters)
1786 1232 {
1787 foreach (OdeCharacter chr in _characters)
1788 {
1789 // Reset the collision values to false
1790 // since we don't know if we're colliding yet
1791 if (chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero)
1792 continue;
1793
1794 chr.IsColliding = false;
1795 chr.CollidingGround = false;
1796 chr.CollidingObj = false;
1797
1798 // Test the avatar's geometry for collision with the space
1799 // This will return near and the space that they are the closest to
1800 // And we'll run this again against the avatar and the space segment
1801 // This will return with a bunch of possible objects in the space segment
1802 // and we'll run it again on all of them.
1803 try 1233 try
1804 { 1234 {
1805 CollideSpaces(space, chr.Shell, IntPtr.Zero); 1235 foreach (OdeCharacter chr in _characters)
1236 {
1237 if (chr == null || chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero)
1238 continue;
1239
1240 chr.IsColliding = false;
1241 // chr.CollidingGround = false; not done here
1242 chr.CollidingObj = false;
1243 // do colisions with static space
1244 d.SpaceCollide2(StaticSpace, chr.Shell, IntPtr.Zero, nearCallback);
1245 // no coll with gnd
1246 }
1806 } 1247 }
1807 catch (AccessViolationException) 1248 catch (AccessViolationException)
1808 { 1249 {
1809 m_log.ErrorFormat("[ODE SCENE]: Unable to space collide {0}", Name); 1250 m_log.Warn("[PHYSICS]: Unable to collide Character to static space");
1810 } 1251 }
1811 1252
1812 //float terrainheight = GetTerrainHeightAtXY(chr.Position.X, chr.Position.Y);
1813 //if (chr.Position.Z + (chr.Velocity.Z * timeStep) < terrainheight + 10)
1814 //{
1815 //chr.Position.Z = terrainheight + 10.0f;
1816 //forcedZ = true;
1817 //}
1818 } 1253 }
1819 1254
1820 if (CollectStats) 1255 lock (_activeprims)
1821 { 1256 {
1822 m_tempAvatarCollisionsThisFrame = _perloopContact.Count; 1257 foreach (OdePrim aprim in _activeprims)
1823 m_stats[ODEAvatarContactsStatsName] += m_tempAvatarCollisionsThisFrame; 1258 {
1259 aprim.CollisionScore = 0;
1260 aprim.IsColliding = false;
1261 }
1824 } 1262 }
1825 1263
1826 List<OdePrim> removeprims = null; 1264 // collide active prims with static enviroment
1827 foreach (OdePrim chr in _activeprims) 1265 lock (_activegroups)
1828 { 1266 {
1829 if (chr.Body != IntPtr.Zero && d.BodyIsEnabled(chr.Body) && (!chr.m_disabled)) 1267 try
1830 { 1268 {
1831 try 1269 foreach (OdePrim prm in _activegroups)
1832 { 1270 {
1833 lock (chr) 1271 if (!prm.m_outbounds)
1834 { 1272 {
1835 if (space != IntPtr.Zero && chr.prim_geom != IntPtr.Zero && chr.m_taintremove == false) 1273 if (d.BodyIsEnabled(prm.Body))
1836 {
1837 CollideSpaces(space, chr.prim_geom, IntPtr.Zero);
1838 }
1839 else
1840 { 1274 {
1841 if (removeprims == null) 1275 d.SpaceCollide2(StaticSpace, prm.collide_geom, IntPtr.Zero, nearCallback);
1842 { 1276 d.SpaceCollide2(GroundSpace, prm.collide_geom, IntPtr.Zero, nearCallback);
1843 removeprims = new List<OdePrim>();
1844 }
1845 removeprims.Add(chr);
1846 m_log.Error(
1847 "[ODE SCENE]: unable to collide test active prim against space. The space was zero, the geom was zero or it was in the process of being removed. Removed it from the active prim list. This needs to be fixed!");
1848 } 1277 }
1849 } 1278 }
1850 } 1279 }
1851 catch (AccessViolationException)
1852 {
1853 m_log.Error("[ODE SCENE]: Unable to space collide");
1854 }
1855 } 1280 }
1856 } 1281 catch (AccessViolationException)
1857
1858 if (CollectStats)
1859 m_stats[ODEPrimContactsStatName] += _perloopContact.Count - m_tempAvatarCollisionsThisFrame;
1860
1861 if (removeprims != null)
1862 {
1863 foreach (OdePrim chr in removeprims)
1864 { 1282 {
1865 _activeprims.Remove(chr); 1283 m_log.Warn("[PHYSICS]: Unable to collide Active prim to static space");
1866 } 1284 }
1867 } 1285 }
1868 } 1286 // finally colide active things amoung them
1869 1287 try
1870 #endregion
1871
1872 public override void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents)
1873 {
1874 m_worldOffset = offset;
1875 WorldExtents = new Vector2(extents.X, extents.Y);
1876 m_parentScene = pScene;
1877 }
1878
1879 // Recovered for use by fly height. Kitto Flora
1880 internal float GetTerrainHeightAtXY(float x, float y)
1881 {
1882 int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
1883 int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
1884
1885 IntPtr heightFieldGeom = IntPtr.Zero;
1886
1887 if (RegionTerrain.TryGetValue(new Vector3(offsetX,offsetY,0), out heightFieldGeom))
1888 { 1288 {
1889 if (heightFieldGeom != IntPtr.Zero) 1289 d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback);
1890 {
1891 if (TerrainHeightFieldHeights.ContainsKey(heightFieldGeom))
1892 {
1893
1894 int index;
1895
1896
1897 if ((int)x > WorldExtents.X || (int)y > WorldExtents.Y ||
1898 (int)x < 0.001f || (int)y < 0.001f)
1899 return 0;
1900
1901 x = x - offsetX;
1902 y = y - offsetY;
1903
1904 index = (int)((int)x * ((int)Constants.RegionSize + 2) + (int)y);
1905
1906 if (index < TerrainHeightFieldHeights[heightFieldGeom].Length)
1907 {
1908 //m_log.DebugFormat("x{0} y{1} = {2}", x, y, (float)TerrainHeightFieldHeights[heightFieldGeom][index]);
1909 return (float)TerrainHeightFieldHeights[heightFieldGeom][index];
1910 }
1911
1912 else
1913 return 0f;
1914 }
1915 else
1916 {
1917 return 0f;
1918 }
1919
1920 }
1921 else
1922 {
1923 return 0f;
1924 }
1925
1926 } 1290 }
1927 else 1291 catch (AccessViolationException)
1928 { 1292 {
1929 return 0f; 1293 m_log.Warn("[PHYSICS]: Unable to collide in Active space");
1930 } 1294 }
1931 } 1295// _perloopContact.Clear();
1932// End recovered. Kitto Flora 1296 }
1933 1297
1298 #endregion
1934 /// <summary> 1299 /// <summary>
1935 /// Add actor to the list that should receive collision events in the simulate loop. 1300 /// Add actor to the list that should receive collision events in the simulate loop.
1936 /// </summary> 1301 /// </summary>
1937 /// <param name="obj"></param> 1302 /// <param name="obj"></param>
1938 internal void AddCollisionEventReporting(PhysicsActor obj) 1303 public void AddCollisionEventReporting(PhysicsActor obj)
1939 { 1304 {
1940// m_log.DebugFormat("[PHYSICS]: Adding {0} {1} to collision event reporting", obj.SOPName, obj.LocalID); 1305 if (!_collisionEventPrim.Contains(obj))
1941 1306 _collisionEventPrim.Add(obj);
1942 lock (m_collisionEventActorsChanges)
1943 m_collisionEventActorsChanges[obj.LocalID] = obj;
1944 } 1307 }
1945 1308
1946 /// <summary> 1309 /// <summary>
1947 /// Remove actor from the list that should receive collision events in the simulate loop. 1310 /// Remove actor from the list that should receive collision events in the simulate loop.
1948 /// </summary> 1311 /// </summary>
1949 /// <param name="obj"></param> 1312 /// <param name="obj"></param>
1950 internal void RemoveCollisionEventReporting(PhysicsActor obj) 1313 public void RemoveCollisionEventReporting(PhysicsActor obj)
1951 { 1314 {
1952// m_log.DebugFormat("[PHYSICS]: Removing {0} {1} from collision event reporting", obj.SOPName, obj.LocalID); 1315 if (_collisionEventPrim.Contains(obj) && !_collisionEventPrimRemove.Contains(obj))
1316 _collisionEventPrimRemove.Add(obj);
1317 }
1953 1318
1954 lock (m_collisionEventActorsChanges) 1319 public override float TimeDilation
1955 m_collisionEventActorsChanges[obj.LocalID] = null; 1320 {
1321 get { return m_timeDilation; }
1322 }
1323
1324 public override bool SupportsNINJAJoints
1325 {
1326 get { return false; }
1956 } 1327 }
1957 1328
1958 #region Add/Remove Entities 1329 #region Add/Remove Entities
@@ -1963,470 +1334,118 @@ namespace OpenSim.Region.Physics.OdePlugin
1963 pos.X = position.X; 1334 pos.X = position.X;
1964 pos.Y = position.Y; 1335 pos.Y = position.Y;
1965 pos.Z = position.Z; 1336 pos.Z = position.Z;
1966 1337 OdeCharacter newAv = new OdeCharacter(avName, this, pos, size, avPIDD, avPIDP, avCapRadius, avDensity, avMovementDivisorWalk, avMovementDivisorRun);
1967 OdeCharacter newAv
1968 = new OdeCharacter(
1969 avName, this, pos, size, avPIDD, avPIDP,
1970 avCapRadius, avStandupTensor, avDensity,
1971 avMovementDivisorWalk, avMovementDivisorRun);
1972
1973 newAv.Flying = isFlying; 1338 newAv.Flying = isFlying;
1974 newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset; 1339 newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset;
1975 1340
1976 return newAv; 1341 return newAv;
1977 } 1342 }
1978 1343
1979 public override void RemoveAvatar(PhysicsActor actor) 1344 public void AddCharacter(OdeCharacter chr)
1980 {
1981// m_log.DebugFormat(
1982// "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2}",
1983// actor.Name, actor.LocalID, Name);
1984
1985 ((OdeCharacter) actor).Destroy();
1986 }
1987
1988 internal void AddCharacter(OdeCharacter chr)
1989 { 1345 {
1990 if (!_characters.Contains(chr)) 1346 lock (_characters)
1991 {
1992 _characters.Add(chr);
1993
1994// m_log.DebugFormat(
1995// "[ODE SCENE]: Adding physics character {0} {1} to physics scene {2}. Count now {3}",
1996// chr.Name, chr.LocalID, Name, _characters.Count);
1997
1998 if (chr.bad)
1999 m_log.ErrorFormat("[ODE SCENE]: Added BAD actor {0} to characters list", chr.m_uuid);
2000 }
2001 else
2002 { 1347 {
2003 m_log.ErrorFormat( 1348 if (!_characters.Contains(chr))
2004 "[ODE SCENE]: Tried to add character {0} {1} but they are already in the set!", 1349 {
2005 chr.Name, chr.LocalID); 1350 _characters.Add(chr);
1351 if (chr.bad)
1352 m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid);
1353 }
2006 } 1354 }
2007 } 1355 }
2008 1356
2009 internal void RemoveCharacter(OdeCharacter chr) 1357 public void RemoveCharacter(OdeCharacter chr)
2010 { 1358 {
2011 if (_characters.Contains(chr)) 1359 lock (_characters)
2012 {
2013 _characters.Remove(chr);
2014
2015// m_log.DebugFormat(
2016// "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2}. Count now {3}",
2017// chr.Name, chr.LocalID, Name, _characters.Count);
2018 }
2019 else
2020 { 1360 {
2021 m_log.ErrorFormat( 1361 if (_characters.Contains(chr))
2022 "[ODE SCENE]: Tried to remove character {0} {1} but they are not in the list!", 1362 {
2023 chr.Name, chr.LocalID); 1363 _characters.Remove(chr);
1364 }
2024 } 1365 }
2025 } 1366 }
2026 1367
2027 private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation, 1368 public void BadCharacter(OdeCharacter chr)
2028 PrimitiveBaseShape pbs, bool isphysical, uint localID)
2029 { 1369 {
2030 Vector3 pos = position; 1370 lock (_badCharacter)
2031 Vector3 siz = size;
2032 Quaternion rot = rotation;
2033
2034 OdePrim newPrim;
2035 lock (OdeLock)
2036 { 1371 {
2037 newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical); 1372 if (!_badCharacter.Contains(chr))
2038 1373 _badCharacter.Add(chr);
2039 lock (_prims)
2040 _prims.Add(newPrim);
2041 } 1374 }
2042 newPrim.LocalID = localID;
2043 return newPrim;
2044 }
2045
2046 /// <summary>
2047 /// Make this prim subject to physics.
2048 /// </summary>
2049 /// <param name="prim"></param>
2050 internal void ActivatePrim(OdePrim prim)
2051 {
2052 // adds active prim.. (ones that should be iterated over in collisions_optimized
2053 if (!_activeprims.Contains(prim))
2054 _activeprims.Add(prim);
2055 //else
2056 // m_log.Warn("[PHYSICS]: Double Entry in _activeprims detected, potential crash immenent");
2057 } 1375 }
2058 1376
2059 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, 1377 public override void RemoveAvatar(PhysicsActor actor)
2060 Vector3 size, Quaternion rotation, bool isPhysical, uint localid)
2061 {
2062// m_log.DebugFormat("[ODE SCENE]: Adding physics prim {0} {1} to physics scene {2}", primName, localid, Name);
2063
2064 return AddPrim(primName, position, size, rotation, pbs, isPhysical, localid);
2065 }
2066
2067 public override float TimeDilation
2068 {
2069 get { return m_timeDilation; }
2070 }
2071
2072 public override bool SupportsNINJAJoints
2073 {
2074 get { return m_NINJA_physics_joints_enabled; }
2075 }
2076
2077 // internal utility function: must be called within a lock (OdeLock)
2078 private void InternalAddActiveJoint(PhysicsJoint joint)
2079 {
2080 activeJoints.Add(joint);
2081 SOPName_to_activeJoint.Add(joint.ObjectNameInScene, joint);
2082 }
2083
2084 // internal utility function: must be called within a lock (OdeLock)
2085 private void InternalAddPendingJoint(OdePhysicsJoint joint)
2086 {
2087 pendingJoints.Add(joint);
2088 SOPName_to_pendingJoint.Add(joint.ObjectNameInScene, joint);
2089 }
2090
2091 // internal utility function: must be called within a lock (OdeLock)
2092 private void InternalRemovePendingJoint(PhysicsJoint joint)
2093 { 1378 {
2094 pendingJoints.Remove(joint); 1379 //m_log.Debug("[PHYSICS]:ODELOCK");
2095 SOPName_to_pendingJoint.Remove(joint.ObjectNameInScene); 1380 ((OdeCharacter) actor).Destroy();
2096 } 1381 }
2097 1382
2098 // internal utility function: must be called within a lock (OdeLock)
2099 private void InternalRemoveActiveJoint(PhysicsJoint joint)
2100 {
2101 activeJoints.Remove(joint);
2102 SOPName_to_activeJoint.Remove(joint.ObjectNameInScene);
2103 }
2104 1383
2105 public override void DumpJointInfo() 1384 public void addActivePrim(OdePrim activatePrim)
2106 { 1385 {
2107 string hdr = "[NINJA] JOINTINFO: "; 1386 // adds active prim..
2108 foreach (PhysicsJoint j in pendingJoints) 1387 lock (_activeprims)
2109 {
2110 m_log.Debug(hdr + " pending joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
2111 }
2112 m_log.Debug(hdr + pendingJoints.Count + " total pending joints");
2113 foreach (string jointName in SOPName_to_pendingJoint.Keys)
2114 {
2115 m_log.Debug(hdr + " pending joints dict contains Name: " + jointName);
2116 }
2117 m_log.Debug(hdr + SOPName_to_pendingJoint.Keys.Count + " total pending joints dict entries");
2118 foreach (PhysicsJoint j in activeJoints)
2119 {
2120 m_log.Debug(hdr + " active joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
2121 }
2122 m_log.Debug(hdr + activeJoints.Count + " total active joints");
2123 foreach (string jointName in SOPName_to_activeJoint.Keys)
2124 {
2125 m_log.Debug(hdr + " active joints dict contains Name: " + jointName);
2126 }
2127 m_log.Debug(hdr + SOPName_to_activeJoint.Keys.Count + " total active joints dict entries");
2128
2129 m_log.Debug(hdr + " Per-body joint connectivity information follows.");
2130 m_log.Debug(hdr + joints_connecting_actor.Keys.Count + " bodies are connected by joints.");
2131 foreach (string actorName in joints_connecting_actor.Keys)
2132 { 1388 {
2133 m_log.Debug(hdr + " Actor " + actorName + " has the following joints connecting it"); 1389 if (!_activeprims.Contains(activatePrim))
2134 foreach (PhysicsJoint j in joints_connecting_actor[actorName]) 1390 _activeprims.Add(activatePrim);
2135 {
2136 m_log.Debug(hdr + " * joint Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
2137 }
2138 m_log.Debug(hdr + joints_connecting_actor[actorName].Count + " connecting joints total for this actor");
2139 } 1391 }
2140 } 1392 }
2141 1393
2142 public override void RequestJointDeletion(string ObjectNameInScene) 1394 public void addActiveGroups(OdePrim activatePrim)
2143 { 1395 {
2144 lock (externalJointRequestsLock) 1396 lock (_activegroups)
2145 { 1397 {
2146 if (!requestedJointsToBeDeleted.Contains(ObjectNameInScene)) // forbid same deletion request from entering twice to prevent spurious deletions processed asynchronously 1398 if (!_activegroups.Contains(activatePrim))
2147 { 1399 _activegroups.Add(activatePrim);
2148 requestedJointsToBeDeleted.Add(ObjectNameInScene);
2149 }
2150 } 1400 }
2151 } 1401 }
2152 1402
2153 private void DeleteRequestedJoints() 1403 private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
1404 PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, byte shapeType, uint localID)
2154 { 1405 {
2155 List<string> myRequestedJointsToBeDeleted; 1406 OdePrim newPrim;
2156 lock (externalJointRequestsLock) 1407 lock (OdeLock)
2157 {
2158 // make a local copy of the shared list for processing (threading issues)
2159 myRequestedJointsToBeDeleted = new List<string>(requestedJointsToBeDeleted);
2160 }
2161
2162 foreach (string jointName in myRequestedJointsToBeDeleted)
2163 {
2164 lock (OdeLock)
2165 {
2166 //m_log.Debug("[NINJA] trying to deleting requested joint " + jointName);
2167 if (SOPName_to_activeJoint.ContainsKey(jointName) || SOPName_to_pendingJoint.ContainsKey(jointName))
2168 {
2169 OdePhysicsJoint joint = null;
2170 if (SOPName_to_activeJoint.ContainsKey(jointName))
2171 {
2172 joint = SOPName_to_activeJoint[jointName] as OdePhysicsJoint;
2173 InternalRemoveActiveJoint(joint);
2174 }
2175 else if (SOPName_to_pendingJoint.ContainsKey(jointName))
2176 {
2177 joint = SOPName_to_pendingJoint[jointName] as OdePhysicsJoint;
2178 InternalRemovePendingJoint(joint);
2179 }
2180
2181 if (joint != null)
2182 {
2183 //m_log.Debug("joint.BodyNames.Count is " + joint.BodyNames.Count + " and contents " + joint.BodyNames);
2184 for (int iBodyName = 0; iBodyName < 2; iBodyName++)
2185 {
2186 string bodyName = joint.BodyNames[iBodyName];
2187 if (bodyName != "NULL")
2188 {
2189 joints_connecting_actor[bodyName].Remove(joint);
2190 if (joints_connecting_actor[bodyName].Count == 0)
2191 {
2192 joints_connecting_actor.Remove(bodyName);
2193 }
2194 }
2195 }
2196
2197 DoJointDeactivated(joint);
2198 if (joint.jointID != IntPtr.Zero)
2199 {
2200 d.JointDestroy(joint.jointID);
2201 joint.jointID = IntPtr.Zero;
2202 //DoJointErrorMessage(joint, "successfully destroyed joint " + jointName);
2203 }
2204 else
2205 {
2206 //m_log.Warn("[NINJA] Ignoring re-request to destroy joint " + jointName);
2207 }
2208 }
2209 else
2210 {
2211 // DoJointErrorMessage(joint, "coult not find joint to destroy based on name " + jointName);
2212 }
2213 }
2214 else
2215 {
2216 // DoJointErrorMessage(joint, "WARNING - joint removal failed, joint " + jointName);
2217 }
2218 }
2219 }
2220
2221 // remove processed joints from the shared list
2222 lock (externalJointRequestsLock)
2223 { 1408 {
2224 foreach (string jointName in myRequestedJointsToBeDeleted) 1409 newPrim = new OdePrim(name, this, position, size, rotation, pbs, isphysical, isPhantom, shapeType, localID);
2225 { 1410 lock (_prims)
2226 requestedJointsToBeDeleted.Remove(jointName); 1411 _prims.Add(newPrim);
2227 }
2228 } 1412 }
1413 return newPrim;
2229 } 1414 }
2230 1415
2231 // for pending joints we don't know if their associated bodies exist yet or not. 1416 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
2232 // the joint is actually created during processing of the taints 1417 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid)
2233 private void CreateRequestedJoints()
2234 { 1418 {
2235 List<PhysicsJoint> myRequestedJointsToBeCreated; 1419 return AddPrim(primName, position, size, rotation, pbs, isPhysical, isPhantom, 0 , localid);
2236 lock (externalJointRequestsLock)
2237 {
2238 // make a local copy of the shared list for processing (threading issues)
2239 myRequestedJointsToBeCreated = new List<PhysicsJoint>(requestedJointsToBeCreated);
2240 }
2241
2242 foreach (PhysicsJoint joint in myRequestedJointsToBeCreated)
2243 {
2244 lock (OdeLock)
2245 {
2246 if (SOPName_to_pendingJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_pendingJoint[joint.ObjectNameInScene] != null)
2247 {
2248 DoJointErrorMessage(joint, "WARNING: ignoring request to re-add already pending joint Name:" + joint.ObjectNameInScene + " type:" + joint.Type + " parms: " + joint.RawParams + " pos: " + joint.Position + " rot:" + joint.Rotation);
2249 continue;
2250 }
2251 if (SOPName_to_activeJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_activeJoint[joint.ObjectNameInScene] != null)
2252 {
2253 DoJointErrorMessage(joint, "WARNING: ignoring request to re-add already active joint Name:" + joint.ObjectNameInScene + " type:" + joint.Type + " parms: " + joint.RawParams + " pos: " + joint.Position + " rot:" + joint.Rotation);
2254 continue;
2255 }
2256
2257 InternalAddPendingJoint(joint as OdePhysicsJoint);
2258
2259 if (joint.BodyNames.Count >= 2)
2260 {
2261 for (int iBodyName = 0; iBodyName < 2; iBodyName++)
2262 {
2263 string bodyName = joint.BodyNames[iBodyName];
2264 if (bodyName != "NULL")
2265 {
2266 if (!joints_connecting_actor.ContainsKey(bodyName))
2267 {
2268 joints_connecting_actor.Add(bodyName, new List<PhysicsJoint>());
2269 }
2270 joints_connecting_actor[bodyName].Add(joint);
2271 }
2272 }
2273 }
2274 }
2275 }
2276
2277 // remove processed joints from shared list
2278 lock (externalJointRequestsLock)
2279 {
2280 foreach (PhysicsJoint joint in myRequestedJointsToBeCreated)
2281 {
2282 requestedJointsToBeCreated.Remove(joint);
2283 }
2284 }
2285 } 1420 }
2286 1421
2287 /// <summary>
2288 /// Add a request for joint creation.
2289 /// </summary>
2290 /// <remarks>
2291 /// this joint will just be added to a waiting list that is NOT processed during the main
2292 /// Simulate() loop (to avoid deadlocks). After Simulate() is finished, we handle unprocessed joint requests.
2293 /// </remarks>
2294 /// <param name="objectNameInScene"></param>
2295 /// <param name="jointType"></param>
2296 /// <param name="position"></param>
2297 /// <param name="rotation"></param>
2298 /// <param name="parms"></param>
2299 /// <param name="bodyNames"></param>
2300 /// <param name="trackedBodyName"></param>
2301 /// <param name="localRotation"></param>
2302 /// <returns></returns>
2303 public override PhysicsJoint RequestJointCreation(
2304 string objectNameInScene, PhysicsJointType jointType, Vector3 position,
2305 Quaternion rotation, string parms, List<string> bodyNames, string trackedBodyName, Quaternion localRotation)
2306 {
2307 OdePhysicsJoint joint = new OdePhysicsJoint();
2308 joint.ObjectNameInScene = objectNameInScene;
2309 joint.Type = jointType;
2310 joint.Position = position;
2311 joint.Rotation = rotation;
2312 joint.RawParams = parms;
2313 joint.BodyNames = new List<string>(bodyNames);
2314 joint.TrackedBodyName = trackedBodyName;
2315 joint.LocalRotation = localRotation;
2316 joint.jointID = IntPtr.Zero;
2317 joint.ErrorMessageCount = 0;
2318
2319 lock (externalJointRequestsLock)
2320 {
2321 if (!requestedJointsToBeCreated.Contains(joint)) // forbid same creation request from entering twice
2322 {
2323 requestedJointsToBeCreated.Add(joint);
2324 }
2325 }
2326
2327 return joint;
2328 }
2329 1422
2330 private void RemoveAllJointsConnectedToActor(PhysicsActor actor) 1423 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1424 Vector3 size, Quaternion rotation, bool isPhysical, uint localid)
2331 { 1425 {
2332 //m_log.Debug("RemoveAllJointsConnectedToActor: start"); 1426 return AddPrim(primName, position, size, rotation, pbs, isPhysical,false, 0, localid);
2333 if (actor.SOPName != null && joints_connecting_actor.ContainsKey(actor.SOPName) && joints_connecting_actor[actor.SOPName] != null)
2334 {
2335 List<PhysicsJoint> jointsToRemove = new List<PhysicsJoint>();
2336 //TODO: merge these 2 loops (originally it was needed to avoid altering a list being iterated over, but it is no longer needed due to the joint request queue mechanism)
2337 foreach (PhysicsJoint j in joints_connecting_actor[actor.SOPName])
2338 {
2339 jointsToRemove.Add(j);
2340 }
2341 foreach (PhysicsJoint j in jointsToRemove)
2342 {
2343 //m_log.Debug("RemoveAllJointsConnectedToActor: about to request deletion of " + j.ObjectNameInScene);
2344 RequestJointDeletion(j.ObjectNameInScene);
2345 //m_log.Debug("RemoveAllJointsConnectedToActor: done request deletion of " + j.ObjectNameInScene);
2346 j.TrackedBodyName = null; // *IMMEDIATELY* prevent any further movement of this joint (else a deleted actor might cause spurious tracking motion of the joint for a few frames, leading to the joint proxy object disappearing)
2347 }
2348 }
2349 } 1427 }
2350 1428
2351 public override void RemoveAllJointsConnectedToActorThreadLocked(PhysicsActor actor) 1429 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1430 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapeType, uint localid)
2352 { 1431 {
2353 //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: start"); 1432
2354 lock (OdeLock) 1433 return AddPrim(primName, position, size, rotation, pbs, isPhysical,isPhantom, shapeType, localid);
2355 {
2356 //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: got lock");
2357 RemoveAllJointsConnectedToActor(actor);
2358 }
2359 } 1434 }
2360 1435
2361 // normally called from within OnJointMoved, which is called from within a lock (OdeLock) 1436 public void remActivePrim(OdePrim deactivatePrim)
2362 public override Vector3 GetJointAnchor(PhysicsJoint joint)
2363 { 1437 {
2364 Debug.Assert(joint.IsInPhysicsEngine); 1438 lock (_activeprims)
2365 d.Vector3 pos = new d.Vector3();
2366
2367 if (!(joint is OdePhysicsJoint))
2368 { 1439 {
2369 DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene); 1440 _activeprims.Remove(deactivatePrim);
2370 } 1441 }
2371 else
2372 {
2373 OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint;
2374 switch (odeJoint.Type)
2375 {
2376 case PhysicsJointType.Ball:
2377 d.JointGetBallAnchor(odeJoint.jointID, out pos);
2378 break;
2379 case PhysicsJointType.Hinge:
2380 d.JointGetHingeAnchor(odeJoint.jointID, out pos);
2381 break;
2382 }
2383 }
2384 return new Vector3(pos.X, pos.Y, pos.Z);
2385 } 1442 }
2386 1443 public void remActiveGroup(OdePrim deactivatePrim)
2387 /// <summary>
2388 /// Get joint axis.
2389 /// </summary>
2390 /// <remarks>
2391 /// normally called from within OnJointMoved, which is called from within a lock (OdeLock)
2392 /// WARNING: ODE sometimes returns <0,0,0> as the joint axis! Therefore this function
2393 /// appears to be unreliable. Fortunately we can compute the joint axis ourselves by
2394 /// keeping track of the joint's original orientation relative to one of the involved bodies.
2395 /// </remarks>
2396 /// <param name="joint"></param>
2397 /// <returns></returns>
2398 public override Vector3 GetJointAxis(PhysicsJoint joint)
2399 { 1444 {
2400 Debug.Assert(joint.IsInPhysicsEngine); 1445 lock (_activegroups)
2401 d.Vector3 axis = new d.Vector3();
2402
2403 if (!(joint is OdePhysicsJoint))
2404 { 1446 {
2405 DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene); 1447 _activegroups.Remove(deactivatePrim);
2406 } 1448 }
2407 else
2408 {
2409 OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint;
2410 switch (odeJoint.Type)
2411 {
2412 case PhysicsJointType.Ball:
2413 DoJointErrorMessage(joint, "warning - axis requested for ball joint: " + joint.ObjectNameInScene);
2414 break;
2415 case PhysicsJointType.Hinge:
2416 d.JointGetHingeAxis(odeJoint.jointID, out axis);
2417 break;
2418 }
2419 }
2420 return new Vector3(axis.X, axis.Y, axis.Z);
2421 }
2422
2423 /// <summary>
2424 /// Stop this prim being subject to physics
2425 /// </summary>
2426 /// <param name="prim"></param>
2427 internal void DeactivatePrim(OdePrim prim)
2428 {
2429 _activeprims.Remove(prim);
2430 } 1449 }
2431 1450
2432 public override void RemovePrim(PhysicsActor prim) 1451 public override void RemovePrim(PhysicsActor prim)
@@ -2435,16 +1454,14 @@ namespace OpenSim.Region.Physics.OdePlugin
2435 // removed in the next physics simulate pass. 1454 // removed in the next physics simulate pass.
2436 if (prim is OdePrim) 1455 if (prim is OdePrim)
2437 { 1456 {
2438 lock (OdeLock) 1457// lock (OdeLock)
2439 { 1458 {
2440 OdePrim p = (OdePrim) prim; 1459
2441 1460 OdePrim p = (OdePrim)prim;
2442 p.setPrimForRemoval(); 1461 p.setPrimForRemoval();
2443 AddPhysicsActorTaint(prim);
2444 } 1462 }
2445 } 1463 }
2446 } 1464 }
2447
2448 /// <summary> 1465 /// <summary>
2449 /// This is called from within simulate but outside the locked portion 1466 /// This is called from within simulate but outside the locked portion
2450 /// We need to do our own locking here 1467 /// We need to do our own locking here
@@ -2457,772 +1474,473 @@ namespace OpenSim.Region.Physics.OdePlugin
2457 /// that the space was using. 1474 /// that the space was using.
2458 /// </summary> 1475 /// </summary>
2459 /// <param name="prim"></param> 1476 /// <param name="prim"></param>
2460 internal void RemovePrimThreadLocked(OdePrim prim) 1477 public void RemovePrimThreadLocked(OdePrim prim)
2461 { 1478 {
2462// m_log.DebugFormat("[ODE SCENE]: Removing physical prim {0} {1}", prim.Name, prim.LocalID); 1479 //Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName);
2463
2464 lock (prim) 1480 lock (prim)
2465 { 1481 {
2466 RemoveCollisionEventReporting(prim); 1482// RemoveCollisionEventReporting(prim);
2467 1483 lock (_prims)
2468 if (prim.prim_geom != IntPtr.Zero) 1484 _prims.Remove(prim);
2469 {
2470 prim.ResetTaints();
2471
2472 if (prim.IsPhysical)
2473 {
2474 prim.disableBody();
2475 if (prim.childPrim)
2476 {
2477 prim.childPrim = false;
2478 prim.Body = IntPtr.Zero;
2479 prim.m_disabled = true;
2480 prim.IsPhysical = false;
2481 }
2482
2483
2484 }
2485 // we don't want to remove the main space
2486
2487 // If the geometry is in the targetspace, remove it from the target space
2488 //m_log.Warn(prim.m_targetSpace);
2489
2490 //if (prim.m_targetSpace != IntPtr.Zero)
2491 //{
2492 //if (d.SpaceQuery(prim.m_targetSpace, prim.prim_geom))
2493 //{
2494
2495 //if (d.GeomIsSpace(prim.m_targetSpace))
2496 //{
2497 //waitForSpaceUnlock(prim.m_targetSpace);
2498 //d.SpaceRemove(prim.m_targetSpace, prim.prim_geom);
2499 prim.m_targetSpace = IntPtr.Zero;
2500 //}
2501 //else
2502 //{
2503 // m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" +
2504 //((OdePrim)prim).m_targetSpace.ToString());
2505 //}
2506
2507 //}
2508 //}
2509 //m_log.Warn(prim.prim_geom);
2510
2511 if (!prim.RemoveGeom())
2512 m_log.Warn("[ODE SCENE]: Unable to remove prim from physics scene");
2513
2514 lock (_prims)
2515 _prims.Remove(prim);
2516
2517 //If there are no more geometries in the sub-space, we don't need it in the main space anymore
2518 //if (d.SpaceGetNumGeoms(prim.m_targetSpace) == 0)
2519 //{
2520 //if (prim.m_targetSpace != null)
2521 //{
2522 //if (d.GeomIsSpace(prim.m_targetSpace))
2523 //{
2524 //waitForSpaceUnlock(prim.m_targetSpace);
2525 //d.SpaceRemove(space, prim.m_targetSpace);
2526 // free up memory used by the space.
2527 //d.SpaceDestroy(prim.m_targetSpace);
2528 //int[] xyspace = calculateSpaceArrayItemFromPos(prim.Position);
2529 //resetSpaceArrayItemToZero(xyspace[0], xyspace[1]);
2530 //}
2531 //else
2532 //{
2533 //m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" +
2534 //((OdePrim) prim).m_targetSpace.ToString());
2535 //}
2536 //}
2537 //}
2538
2539 if (SupportsNINJAJoints)
2540 RemoveAllJointsConnectedToActorThreadLocked(prim);
2541 }
2542 } 1485 }
2543 }
2544 1486
2545 #endregion 1487 }
2546 1488
2547 #region Space Separation Calculation 1489 public bool havePrim(OdePrim prm)
1490 {
1491 lock (_prims)
1492 return _prims.Contains(prm);
1493 }
2548 1494
2549 /// <summary> 1495 public bool haveActor(PhysicsActor actor)
2550 /// Takes a space pointer and zeros out the array we're using to hold the spaces
2551 /// </summary>
2552 /// <param name="pSpace"></param>
2553 private void resetSpaceArrayItemToZero(IntPtr pSpace)
2554 { 1496 {
2555 for (int x = 0; x < staticPrimspace.GetLength(0); x++) 1497 if (actor is OdePrim)
2556 { 1498 {
2557 for (int y = 0; y < staticPrimspace.GetLength(1); y++) 1499 lock (_prims)
2558 { 1500 return _prims.Contains((OdePrim)actor);
2559 if (staticPrimspace[x, y] == pSpace) 1501 }
2560 staticPrimspace[x, y] = IntPtr.Zero; 1502 else if (actor is OdeCharacter)
2561 } 1503 {
1504 lock (_characters)
1505 return _characters.Contains((OdeCharacter)actor);
2562 } 1506 }
1507 return false;
2563 } 1508 }
2564 1509
2565// private void resetSpaceArrayItemToZero(int arrayitemX, int arrayitemY) 1510 #endregion
2566// { 1511
2567// staticPrimspace[arrayitemX, arrayitemY] = IntPtr.Zero; 1512 #region Space Separation Calculation
2568// }
2569 1513
2570 /// <summary> 1514 /// <summary>
2571 /// Called when a static prim moves. Allocates a space for the prim based on its position 1515 /// Called when a static prim moves or becomes static
1516 /// Places the prim in a space one the static sub-spaces grid
2572 /// </summary> 1517 /// </summary>
2573 /// <param name="geom">the pointer to the geom that moved</param> 1518 /// <param name="geom">the pointer to the geom that moved</param>
2574 /// <param name="pos">the position that the geom moved to</param> 1519 /// <param name="pos">the position that the geom moved to</param>
2575 /// <param name="currentspace">a pointer to the space it was in before it was moved.</param> 1520 /// <param name="currentspace">a pointer to the space it was in before it was moved.</param>
2576 /// <returns>a pointer to the new space it's in</returns> 1521 /// <returns>a pointer to the new space it's in</returns>
2577 internal IntPtr recalculateSpaceForGeom(IntPtr geom, Vector3 pos, IntPtr currentspace) 1522 public IntPtr MoveGeomToStaticSpace(IntPtr geom, Vector3 pos, IntPtr currentspace)
2578 { 1523 {
2579 // Called from setting the Position and Size of an ODEPrim so 1524 // moves a prim into another static sub-space or from another space into a static sub-space
1525
1526 // Called ODEPrim so
2580 // it's already in locked space. 1527 // it's already in locked space.
2581 1528
2582 // we don't want to remove the main space 1529 if (geom == IntPtr.Zero) // shouldn't happen
2583 // we don't need to test physical here because this function should 1530 return IntPtr.Zero;
2584 // never be called if the prim is physical(active)
2585 1531
2586 // All physical prim end up in the root space 1532 // get the static sub-space for current position
2587 //Thread.Sleep(20); 1533 IntPtr newspace = calculateSpaceForGeom(pos);
2588 if (currentspace != space) 1534
1535 if (newspace == currentspace) // if we are there all done
1536 return newspace;
1537
1538 // else remove it from its current space
1539 if (currentspace != IntPtr.Zero && d.SpaceQuery(currentspace, geom))
2589 { 1540 {
2590 //m_log.Info("[SPACE]: C:" + currentspace.ToString() + " g:" + geom.ToString()); 1541 if (d.GeomIsSpace(currentspace))
2591 //if (currentspace == IntPtr.Zero)
2592 //{
2593 //int adfadf = 0;
2594 //}
2595 if (d.SpaceQuery(currentspace, geom) && currentspace != IntPtr.Zero)
2596 { 1542 {
2597 if (d.GeomIsSpace(currentspace)) 1543 waitForSpaceUnlock(currentspace);
2598 { 1544 d.SpaceRemove(currentspace, geom);
2599// waitForSpaceUnlock(currentspace); 1545
2600 d.SpaceRemove(currentspace, geom); 1546 if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
2601 }
2602 else
2603 { 1547 {
2604 m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" + currentspace + 1548 d.SpaceDestroy(currentspace);
2605 " Geom:" + geom);
2606 } 1549 }
2607 } 1550 }
2608 else 1551 else
2609 { 1552 {
2610 IntPtr sGeomIsIn = d.GeomGetSpace(geom); 1553 m_log.Info("[Physics]: Invalid or empty Space passed to 'MoveGeomToStaticSpace':" + currentspace +
2611 if (sGeomIsIn != IntPtr.Zero) 1554 " Geom:" + geom);
2612 {
2613 if (d.GeomIsSpace(currentspace))
2614 {
2615// waitForSpaceUnlock(sGeomIsIn);
2616 d.SpaceRemove(sGeomIsIn, geom);
2617 }
2618 else
2619 {
2620 m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" +
2621 sGeomIsIn + " Geom:" + geom);
2622 }
2623 }
2624 }
2625
2626 //If there are no more geometries in the sub-space, we don't need it in the main space anymore
2627 if (d.SpaceGetNumGeoms(currentspace) == 0)
2628 {
2629 if (currentspace != IntPtr.Zero)
2630 {
2631 if (d.GeomIsSpace(currentspace))
2632 {
2633// waitForSpaceUnlock(currentspace);
2634// waitForSpaceUnlock(space);
2635 d.SpaceRemove(space, currentspace);
2636 // free up memory used by the space.
2637
2638 //d.SpaceDestroy(currentspace);
2639 resetSpaceArrayItemToZero(currentspace);
2640 }
2641 else
2642 {
2643 m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" +
2644 currentspace + " Geom:" + geom);
2645 }
2646 }
2647 } 1555 }
2648 } 1556 }
2649 else 1557 else // odd currentspace is null or doesn't contain the geom? lets try the geom ideia of current space
2650 { 1558 {
2651 // this is a physical object that got disabled. ;.; 1559 currentspace = d.GeomGetSpace(geom);
2652 if (currentspace != IntPtr.Zero && geom != IntPtr.Zero) 1560 if (currentspace != IntPtr.Zero)
2653 { 1561 {
2654 if (d.SpaceQuery(currentspace, geom)) 1562 if (d.GeomIsSpace(currentspace))
2655 {
2656 if (d.GeomIsSpace(currentspace))
2657 {
2658// waitForSpaceUnlock(currentspace);
2659 d.SpaceRemove(currentspace, geom);
2660 }
2661 else
2662 {
2663 m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" +
2664 currentspace + " Geom:" + geom);
2665 }
2666 }
2667 else
2668 { 1563 {
2669 IntPtr sGeomIsIn = d.GeomGetSpace(geom); 1564 waitForSpaceUnlock(currentspace);
2670 if (sGeomIsIn != IntPtr.Zero) 1565 d.SpaceRemove(currentspace, geom);
1566
1567 if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
2671 { 1568 {
2672 if (d.GeomIsSpace(sGeomIsIn)) 1569 d.SpaceDestroy(currentspace);
2673 {
2674// waitForSpaceUnlock(sGeomIsIn);
2675 d.SpaceRemove(sGeomIsIn, geom);
2676 }
2677 else
2678 {
2679 m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" +
2680 sGeomIsIn + " Geom:" + geom);
2681 }
2682 } 1570 }
1571
2683 } 1572 }
2684 } 1573 }
2685 } 1574 }
2686 1575
2687 // The routines in the Position and Size sections do the 'inserting' into the space, 1576 // put the geom in the newspace
2688 // so all we have to do is make sure that the space that we're putting the prim into 1577 waitForSpaceUnlock(newspace);
2689 // is in the 'main' space. 1578 d.SpaceAdd(newspace, geom);
2690 int[] iprimspaceArrItem = calculateSpaceArrayItemFromPos(pos);
2691 IntPtr newspace = calculateSpaceForGeom(pos);
2692
2693 if (newspace == IntPtr.Zero)
2694 {
2695 newspace = createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
2696 d.HashSpaceSetLevels(newspace, smallHashspaceLow, smallHashspaceHigh);
2697 }
2698 1579
1580 // let caller know this newspace
2699 return newspace; 1581 return newspace;
2700 } 1582 }
2701 1583
2702 /// <summary> 1584 /// <summary>
2703 /// Creates a new space at X Y
2704 /// </summary>
2705 /// <param name="iprimspaceArrItemX"></param>
2706 /// <param name="iprimspaceArrItemY"></param>
2707 /// <returns>A pointer to the created space</returns>
2708 internal IntPtr createprimspace(int iprimspaceArrItemX, int iprimspaceArrItemY)
2709 {
2710 // creating a new space for prim and inserting it into main space.
2711 staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY] = d.HashSpaceCreate(IntPtr.Zero);
2712 d.GeomSetCategoryBits(staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY], (int)CollisionCategories.Space);
2713// waitForSpaceUnlock(space);
2714 d.SpaceSetSublevel(space, 1);
2715 d.SpaceAdd(space, staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY]);
2716
2717 return staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY];
2718 }
2719
2720 /// <summary>
2721 /// Calculates the space the prim should be in by its position 1585 /// Calculates the space the prim should be in by its position
2722 /// </summary> 1586 /// </summary>
2723 /// <param name="pos"></param> 1587 /// <param name="pos"></param>
2724 /// <returns>a pointer to the space. This could be a new space or reused space.</returns> 1588 /// <returns>a pointer to the space. This could be a new space or reused space.</returns>
2725 internal IntPtr calculateSpaceForGeom(Vector3 pos) 1589 public IntPtr calculateSpaceForGeom(Vector3 pos)
2726 {
2727 int[] xyspace = calculateSpaceArrayItemFromPos(pos);
2728 //m_log.Info("[Physics]: Attempting to use arrayItem: " + xyspace[0].ToString() + "," + xyspace[1].ToString());
2729 return staticPrimspace[xyspace[0], xyspace[1]];
2730 }
2731
2732 /// <summary>
2733 /// Holds the space allocation logic
2734 /// </summary>
2735 /// <param name="pos"></param>
2736 /// <returns>an array item based on the position</returns>
2737 internal int[] calculateSpaceArrayItemFromPos(Vector3 pos)
2738 { 1590 {
2739 int[] returnint = new int[2]; 1591 int x, y;
2740 1592
2741 returnint[0] = (int) (pos.X/metersInSpace); 1593 if (pos.X < 0)
1594 return staticPrimspaceOffRegion[0];
2742 1595
2743 if (returnint[0] > ((int) (259f/metersInSpace))) 1596 if (pos.Y < 0)
2744 returnint[0] = ((int) (259f/metersInSpace)); 1597 return staticPrimspaceOffRegion[2];
2745 if (returnint[0] < 0)
2746 returnint[0] = 0;
2747 1598
2748 returnint[1] = (int) (pos.Y/metersInSpace); 1599 x = (int)(pos.X * spacesPerMeter);
2749 if (returnint[1] > ((int) (259f/metersInSpace))) 1600 if (x > spaceGridMaxX)
2750 returnint[1] = ((int) (259f/metersInSpace)); 1601 return staticPrimspaceOffRegion[1];
2751 if (returnint[1] < 0) 1602
2752 returnint[1] = 0; 1603 y = (int)(pos.Y * spacesPerMeter);
1604 if (y > spaceGridMaxY)
1605 return staticPrimspaceOffRegion[3];
2753 1606
2754 return returnint; 1607 return staticPrimspace[x, y];
2755 } 1608 }
2756 1609
2757 #endregion 1610 #endregion
2758 1611
1612
2759 /// <summary> 1613 /// <summary>
2760 /// Routine to figure out if we need to mesh this prim with our mesher 1614 /// Called to queue a change to a actor
1615 /// to use in place of old taint mechanism so changes do have a time sequence
2761 /// </summary> 1616 /// </summary>
2762 /// <param name="pbs"></param>
2763 /// <returns></returns>
2764 internal bool needsMeshing(PrimitiveBaseShape pbs)
2765 {
2766 // most of this is redundant now as the mesher will return null if it cant mesh a prim
2767 // but we still need to check for sculptie meshing being enabled so this is the most
2768 // convenient place to do it for now...
2769
2770 // //if (pbs.PathCurve == (byte)Primitive.PathCurve.Circle && pbs.ProfileCurve == (byte)Primitive.ProfileCurve.Circle && pbs.PathScaleY <= 0.75f)
2771 // //m_log.Debug("needsMeshing: " + " pathCurve: " + pbs.PathCurve.ToString() + " profileCurve: " + pbs.ProfileCurve.ToString() + " pathScaleY: " + Primitive.UnpackPathScale(pbs.PathScaleY).ToString());
2772 int iPropertiesNotSupportedDefault = 0;
2773
2774 if (pbs.SculptEntry && !meshSculptedPrim)
2775 {
2776#if SPAM
2777 m_log.Warn("NonMesh");
2778#endif
2779 return false;
2780 }
2781
2782 // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since ODE can use an internal representation for the prim
2783 if (!forceSimplePrimMeshing && !pbs.SculptEntry)
2784 {
2785 if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
2786 || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1
2787 && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z))
2788 {
2789
2790 if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0
2791 && pbs.ProfileHollow == 0
2792 && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0
2793 && pbs.PathBegin == 0 && pbs.PathEnd == 0
2794 && pbs.PathTaperX == 0 && pbs.PathTaperY == 0
2795 && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
2796 && pbs.PathShearX == 0 && pbs.PathShearY == 0)
2797 {
2798#if SPAM
2799 m_log.Warn("NonMesh");
2800#endif
2801 return false;
2802 }
2803 }
2804 }
2805
2806 if (pbs.ProfileHollow != 0)
2807 iPropertiesNotSupportedDefault++;
2808 1617
2809 if ((pbs.PathBegin != 0) || pbs.PathEnd != 0) 1618 public void AddChange(PhysicsActor actor, changes what, Object arg)
2810 iPropertiesNotSupportedDefault++; 1619 {
2811 1620 ODEchangeitem item = new ODEchangeitem();
2812 if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0)) 1621 item.actor = actor;
2813 iPropertiesNotSupportedDefault++; 1622 item.what = what;
2814 1623 item.arg = arg;
2815 if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0) 1624 ChangesQueue.Enqueue(item);
2816 iPropertiesNotSupportedDefault++;
2817
2818 if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100))
2819 iPropertiesNotSupportedDefault++;
2820
2821 if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0))
2822 iPropertiesNotSupportedDefault++;
2823
2824 if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight)
2825 iPropertiesNotSupportedDefault++;
2826
2827 if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 && (pbs.Scale.X != pbs.Scale.Y || pbs.Scale.Y != pbs.Scale.Z || pbs.Scale.Z != pbs.Scale.X))
2828 iPropertiesNotSupportedDefault++;
2829
2830 if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1)
2831 iPropertiesNotSupportedDefault++;
2832
2833 // test for torus
2834 if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square)
2835 {
2836 if (pbs.PathCurve == (byte)Extrusion.Curve1)
2837 {
2838 iPropertiesNotSupportedDefault++;
2839 }
2840 }
2841 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle)
2842 {
2843 if (pbs.PathCurve == (byte)Extrusion.Straight)
2844 {
2845 iPropertiesNotSupportedDefault++;
2846 }
2847
2848 // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits
2849 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
2850 {
2851 iPropertiesNotSupportedDefault++;
2852 }
2853 }
2854 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle)
2855 {
2856 if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2)
2857 {
2858 iPropertiesNotSupportedDefault++;
2859 }
2860 }
2861 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle)
2862 {
2863 if (pbs.PathCurve == (byte)Extrusion.Straight)
2864 {
2865 iPropertiesNotSupportedDefault++;
2866 }
2867 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
2868 {
2869 iPropertiesNotSupportedDefault++;
2870 }
2871 }
2872
2873 if (pbs.SculptEntry && meshSculptedPrim)
2874 iPropertiesNotSupportedDefault++;
2875
2876 if (iPropertiesNotSupportedDefault == 0)
2877 {
2878#if SPAM
2879 m_log.Warn("NonMesh");
2880#endif
2881 return false;
2882 }
2883#if SPAM
2884 m_log.Debug("Mesh");
2885#endif
2886 return true;
2887 } 1625 }
2888 1626
2889 /// <summary> 1627 /// <summary>
2890 /// Called after our prim properties are set Scale, position etc. 1628 /// Called after our prim properties are set Scale, position etc.
2891 /// </summary>
2892 /// <remarks>
2893 /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex 1629 /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex
2894 /// This assures us that we have no race conditions 1630 /// This assures us that we have no race conditions
2895 /// </remarks> 1631 /// </summary>
2896 /// <param name="actor"></param> 1632 /// <param name="prim"></param>
2897 public override void AddPhysicsActorTaint(PhysicsActor actor) 1633 public override void AddPhysicsActorTaint(PhysicsActor prim)
2898 { 1634 {
2899 if (actor is OdePrim) 1635 }
2900 { 1636
2901 OdePrim taintedprim = ((OdePrim)actor); 1637 // does all pending changes generated during region load process
2902 lock (_taintedPrims) 1638 public override void PrepareSimulation()
2903 _taintedPrims.Add(taintedprim); 1639 {
2904 } 1640 lock (OdeLock)
2905 else if (actor is OdeCharacter)
2906 { 1641 {
2907 OdeCharacter taintedchar = ((OdeCharacter)actor); 1642 if (world == IntPtr.Zero)
2908 lock (_taintedActors)
2909 { 1643 {
2910 _taintedActors.Add(taintedchar); 1644 ChangesQueue.Clear();
2911 if (taintedchar.bad) 1645 return;
2912 m_log.ErrorFormat("[ODE SCENE]: Added BAD actor {0} to tainted actors", taintedchar.m_uuid); 1646 }
1647
1648 ODEchangeitem item;
1649
1650 int donechanges = 0;
1651 if (ChangesQueue.Count > 0)
1652 {
1653 m_log.InfoFormat("[ODE] start processing pending actor operations");
1654 int tstart = Util.EnvironmentTickCount();
1655
1656 while (ChangesQueue.Dequeue(out item))
1657 {
1658 if (item.actor != null)
1659 {
1660 try
1661 {
1662 if (item.actor is OdeCharacter)
1663 ((OdeCharacter)item.actor).DoAChange(item.what, item.arg);
1664 else if (((OdePrim)item.actor).DoAChange(item.what, item.arg))
1665 RemovePrimThreadLocked((OdePrim)item.actor);
1666 }
1667 catch
1668 {
1669 m_log.WarnFormat("[PHYSICS]: Operation failed for a actor {0} {1}",
1670 item.actor.Name, item.what.ToString());
1671 }
1672 }
1673 donechanges++;
1674 }
1675 int time = Util.EnvironmentTickCountSubtract(tstart);
1676 m_log.InfoFormat("[ODE] finished {0} operations in {1}ms", donechanges, time);
2913 } 1677 }
2914 } 1678 }
2915 } 1679 }
2916 1680
2917 /// <summary> 1681 /// <summary>
2918 /// This is our main simulate loop 1682 /// This is our main simulate loop
2919 /// </summary>
2920 /// <remarks>
2921 /// It's thread locked by a Mutex in the scene. 1683 /// It's thread locked by a Mutex in the scene.
2922 /// It holds Collisions, it instructs ODE to step through the physical reactions 1684 /// It holds Collisions, it instructs ODE to step through the physical reactions
2923 /// It moves the objects around in memory 1685 /// It moves the objects around in memory
2924 /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup) 1686 /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup)
2925 /// </remarks> 1687 /// </summary>
2926 /// <param name="timeStep"></param> 1688 /// <param name="timeStep"></param>
2927 /// <returns>The number of frames simulated over that period.</returns> 1689 /// <returns></returns>
2928 public override float Simulate(float timeStep) 1690 public override float Simulate(float timeStep)
2929 { 1691 {
2930 if (!_worldInitialized) return 11f;
2931 1692
2932 int startFrameTick = CollectStats ? Util.EnvironmentTickCount() : 0; 1693 DateTime now = DateTime.UtcNow;
2933 int tempTick = 0, tempTick2 = 0; 1694 TimeSpan timedif = now - m_lastframe;
1695 m_lastframe = now;
1696 timeStep = (float)timedif.TotalSeconds;
1697
1698 // acumulate time so we can reduce error
1699 step_time += timeStep;
1700
1701 if (step_time < HalfOdeStep)
1702 return 0;
2934 1703
2935 if (framecount >= int.MaxValue) 1704 if (framecount < 0)
2936 framecount = 0; 1705 framecount = 0;
2937 1706
2938 framecount++; 1707 framecount++;
2939 1708
2940 float fps = 0; 1709 int curphysiteractions;
2941 1710
2942 float timeLeft = timeStep; 1711 // if in trouble reduce step resolution
2943 1712 if (step_time >= m_SkipFramesAtms)
2944 //m_log.Info(timeStep.ToString()); 1713 curphysiteractions = m_physicsiterations / 2;
2945// step_time += timeSte 1714 else
2946// 1715 curphysiteractions = m_physicsiterations;
2947// // If We're loaded down by something else,
2948// // or debugging with the Visual Studio project on pause
2949// // skip a few frames to catch up gracefully.
2950// // without shooting the physicsactors all over the place
2951//
2952// if (step_time >= m_SkipFramesAtms)
2953// {
2954// // Instead of trying to catch up, it'll do 5 physics frames only
2955// step_time = ODE_STEPSIZE;
2956// m_physicsiterations = 5;
2957// }
2958// else
2959// {
2960// m_physicsiterations = 10;
2961// }
2962
2963 // We change _collisionEventPrimChanges to avoid locking _collisionEventPrim itself and causing potential
2964 // deadlock if the collision event tries to lock something else later on which is already locked by a
2965 // caller that is adding or removing the collision event.
2966 lock (m_collisionEventActorsChanges)
2967 {
2968 foreach (KeyValuePair<uint, PhysicsActor> kvp in m_collisionEventActorsChanges)
2969 {
2970 if (kvp.Value == null)
2971 m_collisionEventActors.Remove(kvp.Key);
2972 else
2973 m_collisionEventActors[kvp.Key] = kvp.Value;
2974 }
2975 1716
2976 m_collisionEventActorsChanges.Clear(); 1717 int nodeframes = 0;
2977 }
2978 1718
2979 if (SupportsNINJAJoints) 1719// checkThread();
2980 {
2981 DeleteRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks
2982 CreateRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks
2983 }
2984 1720
2985 lock (OdeLock) 1721 lock (SimulationLock)
1722 lock(OdeLock)
2986 { 1723 {
2987 // Process 10 frames if the sim is running normal.. 1724 if (world == IntPtr.Zero)
2988 // process 5 frames if the sim is running slow
2989 //try
2990 //{
2991 //d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
2992 //}
2993 //catch (StackOverflowException)
2994 //{
2995 // m_log.Error("[PHYSICS]: The operating system wasn't able to allocate enough memory for the simulation. Restarting the sim.");
2996 // ode.drelease(world);
2997 //base.TriggerPhysicsBasedRestart();
2998 //}
2999
3000 // Figure out the Frames Per Second we're going at.
3001 //(step_time == 0.004f, there's 250 of those per second. Times the step time/step size
3002
3003 fps = (timeStep / ODE_STEPSIZE) * 1000;
3004 // HACK: Using a time dilation of 1.0 to debug rubberbanding issues
3005 //m_timeDilation = Math.Min((step_time / ODE_STEPSIZE) / (0.09375f / ODE_STEPSIZE), 1.0f);
3006
3007 while (timeLeft > 0.0f)
3008 { 1725 {
3009 try 1726 ChangesQueue.Clear();
3010 { 1727 return 0;
3011 if (CollectStats) 1728 }
3012 tempTick = Util.EnvironmentTickCount();
3013
3014 lock (_taintedActors)
3015 {
3016 foreach (OdeCharacter character in _taintedActors)
3017 character.ProcessTaints();
3018 1729
3019 _taintedActors.Clear(); 1730 ODEchangeitem item;
3020 }
3021 1731
3022 if (CollectStats) 1732 if (ChangesQueue.Count > 0)
3023 { 1733 {
3024 tempTick2 = Util.EnvironmentTickCount(); 1734 int ttmpstart = Util.EnvironmentTickCount();
3025 m_stats[ODEAvatarTaintMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick); 1735 int ttmp;
3026 tempTick = tempTick2;
3027 }
3028 1736
3029 lock (_taintedPrims) 1737 while (ChangesQueue.Dequeue(out item))
1738 {
1739 if (item.actor != null)
3030 { 1740 {
3031 foreach (OdePrim prim in _taintedPrims) 1741 try
3032 { 1742 {
3033 if (prim.m_taintremove) 1743 if (item.actor is OdeCharacter)
3034 { 1744 ((OdeCharacter)item.actor).DoAChange(item.what, item.arg);
3035// Console.WriteLine("Simulate calls RemovePrimThreadLocked for {0}", prim.Name); 1745 else if (((OdePrim)item.actor).DoAChange(item.what, item.arg))
3036 RemovePrimThreadLocked(prim); 1746 RemovePrimThreadLocked((OdePrim)item.actor);
3037 } 1747 }
3038 else 1748 catch
3039 { 1749 {
3040// Console.WriteLine("Simulate calls ProcessTaints for {0}", prim.Name); 1750 m_log.WarnFormat("[PHYSICS]: doChange failed for a actor {0} {1}",
3041 prim.ProcessTaints(); 1751 item.actor.Name, item.what.ToString());
3042 }
3043
3044 prim.m_collisionscore = 0;
3045
3046 // This loop can block up the Heartbeat for a very long time on large regions.
3047 // We need to let the Watchdog know that the Heartbeat is not dead
3048 // NOTE: This is currently commented out, but if things like OAR loading are
3049 // timing the heartbeat out we will need to uncomment it
3050 //Watchdog.UpdateThread();
3051 } 1752 }
3052
3053 if (SupportsNINJAJoints)
3054 SimulatePendingNINJAJoints();
3055
3056 _taintedPrims.Clear();
3057 } 1753 }
1754 ttmp = Util.EnvironmentTickCountSubtract(ttmpstart);
1755 if (ttmp > 20)
1756 break;
1757 }
1758 }
1759
1760 d.WorldSetQuickStepNumIterations(world, curphysiteractions);
3058 1761
3059 if (CollectStats) 1762 while (step_time > HalfOdeStep && nodeframes < 10) //limit number of steps so we don't say here for ever
3060 { 1763 {
3061 tempTick2 = Util.EnvironmentTickCount(); 1764 try
3062 m_stats[ODEPrimTaintMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick); 1765 {
3063 tempTick = tempTick2; 1766 // clear pointer/counter to contacts to pass into joints
3064 } 1767 m_global_contactcount = 0;
3065 1768
3066 // Move characters
3067 foreach (OdeCharacter actor in _characters)
3068 actor.Move(defects);
3069 1769
3070 if (defects.Count != 0) 1770 // Move characters
1771 lock (_characters)
3071 { 1772 {
3072 foreach (OdeCharacter actor in defects) 1773 List<OdeCharacter> defects = new List<OdeCharacter>();
1774 foreach (OdeCharacter actor in _characters)
3073 { 1775 {
3074 m_log.ErrorFormat( 1776 if (actor != null)
3075 "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2} due to defect found when moving", 1777 actor.Move(ODE_STEPSIZE, defects);
3076 actor.Name, actor.LocalID, Name); 1778 }
3077 1779 if (defects.Count != 0)
3078 RemoveCharacter(actor); 1780 {
3079 actor.DestroyOdeStructures(); 1781 foreach (OdeCharacter defect in defects)
1782 {
1783 RemoveCharacter(defect);
1784 }
1785 defects.Clear();
3080 } 1786 }
3081
3082 defects.Clear();
3083 }
3084
3085 if (CollectStats)
3086 {
3087 tempTick2 = Util.EnvironmentTickCount();
3088 m_stats[ODEAvatarForcesFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
3089 tempTick = tempTick2;
3090 } 1787 }
3091 1788
3092 // Move other active objects 1789 // Move other active objects
3093 foreach (OdePrim prim in _activeprims) 1790 lock (_activegroups)
3094 {
3095 prim.m_collisionscore = 0;
3096 prim.Move(timeStep);
3097 }
3098
3099 if (CollectStats)
3100 { 1791 {
3101 tempTick2 = Util.EnvironmentTickCount(); 1792 foreach (OdePrim aprim in _activegroups)
3102 m_stats[ODEPrimForcesFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick); 1793 {
3103 tempTick = tempTick2; 1794 aprim.Move();
1795 }
3104 } 1796 }
3105 1797
3106 //if ((framecount % m_randomizeWater) == 0) 1798 //if ((framecount % m_randomizeWater) == 0)
3107 // randomizeWater(waterlevel); 1799 // randomizeWater(waterlevel);
3108 1800
3109 //int RayCastTimeMS = m_rayCastManager.ProcessQueuedRequests();
3110 m_rayCastManager.ProcessQueuedRequests(); 1801 m_rayCastManager.ProcessQueuedRequests();
3111 1802
3112 if (CollectStats)
3113 {
3114 tempTick2 = Util.EnvironmentTickCount();
3115 m_stats[ODERaycastingFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
3116 tempTick = tempTick2;
3117 }
3118
3119 collision_optimized(); 1803 collision_optimized();
3120 1804
3121 if (CollectStats) 1805 foreach (PhysicsActor obj in _collisionEventPrim)
3122 {
3123 tempTick2 = Util.EnvironmentTickCount();
3124 m_stats[ODEOtherCollisionFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
3125 tempTick = tempTick2;
3126 }
3127
3128 foreach (PhysicsActor obj in m_collisionEventActors.Values)
3129 { 1806 {
3130// m_log.DebugFormat("[PHYSICS]: Assessing {0} {1} for collision events", obj.SOPName, obj.LocalID); 1807 if (obj == null)
1808 continue;
3131 1809
3132 switch ((ActorTypes)obj.PhysicsActorType) 1810 switch ((ActorTypes)obj.PhysicsActorType)
3133 { 1811 {
3134 case ActorTypes.Agent: 1812 case ActorTypes.Agent:
3135 OdeCharacter cobj = (OdeCharacter)obj; 1813 OdeCharacter cobj = (OdeCharacter)obj;
3136 cobj.AddCollisionFrameTime(100); 1814 cobj.AddCollisionFrameTime((int)(odetimestepMS));
3137 cobj.SendCollisions(); 1815 cobj.SendCollisions();
3138 break; 1816 break;
3139 1817
3140 case ActorTypes.Prim: 1818 case ActorTypes.Prim:
3141 OdePrim pobj = (OdePrim)obj; 1819 OdePrim pobj = (OdePrim)obj;
3142 pobj.SendCollisions(); 1820 if (pobj.Body == IntPtr.Zero || (d.BodyIsEnabled(pobj.Body) && !pobj.m_outbounds))
1821 if (!pobj.m_outbounds)
1822 {
1823 pobj.AddCollisionFrameTime((int)(odetimestepMS));
1824 pobj.SendCollisions();
1825 }
3143 break; 1826 break;
3144 } 1827 }
3145 } 1828 }
3146 1829
3147// if (m_global_contactcount > 0) 1830 foreach (PhysicsActor obj in _collisionEventPrimRemove)
3148// m_log.DebugFormat( 1831 _collisionEventPrim.Remove(obj);
3149// "[PHYSICS]: Collision contacts to process this frame = {0}", m_global_contactcount);
3150
3151 m_global_contactcount = 0;
3152 1832
3153 if (CollectStats) 1833 _collisionEventPrimRemove.Clear();
3154 {
3155 tempTick2 = Util.EnvironmentTickCount();
3156 m_stats[ODECollisionNotificationFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
3157 tempTick = tempTick2;
3158 }
3159 1834
1835 // do a ode simulation step
3160 d.WorldQuickStep(world, ODE_STEPSIZE); 1836 d.WorldQuickStep(world, ODE_STEPSIZE);
1837 d.JointGroupEmpty(contactgroup);
3161 1838
3162 if (CollectStats) 1839 // update managed ideia of physical data and do updates to core
3163 m_stats[ODENativeStepFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick); 1840 /*
1841 lock (_characters)
1842 {
1843 foreach (OdeCharacter actor in _characters)
1844 {
1845 if (actor != null)
1846 {
1847 if (actor.bad)
1848 m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
3164 1849
3165 d.JointGroupEmpty(contactgroup); 1850 actor.UpdatePositionAndVelocity();
1851 }
1852 }
1853 }
1854 */
1855
1856 lock (_activegroups)
1857 {
1858 {
1859 foreach (OdePrim actor in _activegroups)
1860 {
1861 if (actor.IsPhysical)
1862 {
1863 actor.UpdatePositionAndVelocity();
1864 }
1865 }
1866 }
1867 }
3166 } 1868 }
3167 catch (Exception e) 1869 catch (Exception e)
3168 { 1870 {
3169 m_log.ErrorFormat("[ODE SCENE]: {0}, {1}, {2}", e.Message, e.TargetSite, e); 1871 m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
1872// ode.dunlock(world);
3170 } 1873 }
3171 1874
3172 timeLeft -= ODE_STEPSIZE;
3173 }
3174 1875
3175 if (CollectStats) 1876 step_time -= ODE_STEPSIZE;
3176 tempTick = Util.EnvironmentTickCount(); 1877 nodeframes++;
3177
3178 foreach (OdeCharacter actor in _characters)
3179 {
3180 if (actor.bad)
3181 m_log.ErrorFormat("[ODE SCENE]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
3182
3183 actor.UpdatePositionAndVelocity(defects);
3184 } 1878 }
3185 1879
3186 if (defects.Count != 0) 1880 lock (_badCharacter)
3187 { 1881 {
3188 foreach (OdeCharacter actor in defects) 1882 if (_badCharacter.Count > 0)
3189 { 1883 {
3190 m_log.ErrorFormat( 1884 foreach (OdeCharacter chr in _badCharacter)
3191 "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2} due to defect found when updating position and velocity", 1885 {
3192 actor.Name, actor.LocalID, Name); 1886 RemoveCharacter(chr);
1887 }
3193 1888
3194 RemoveCharacter(actor); 1889 _badCharacter.Clear();
3195 actor.DestroyOdeStructures();
3196 } 1890 }
3197
3198 defects.Clear();
3199 } 1891 }
3200 1892
3201 if (CollectStats) 1893 timedif = now - m_lastMeshExpire;
1894
1895 if (timedif.Seconds > 10)
3202 { 1896 {
3203 tempTick2 = Util.EnvironmentTickCount(); 1897 mesher.ExpireReleaseMeshs();
3204 m_stats[ODEAvatarUpdateFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick); 1898 m_lastMeshExpire = now;
3205 tempTick = tempTick2;
3206 } 1899 }
3207 1900
3208 //if (timeStep < 0.2f) 1901// information block running in debug only
1902/*
1903 int ntopactivegeoms = d.SpaceGetNumGeoms(ActiveSpace);
1904 int ntopstaticgeoms = d.SpaceGetNumGeoms(StaticSpace);
1905 int ngroundgeoms = d.SpaceGetNumGeoms(GroundSpace);
1906
1907 int nactivegeoms = 0;
1908 int nactivespaces = 0;
3209 1909
3210 foreach (OdePrim prim in _activeprims) 1910 int nstaticgeoms = 0;
1911 int nstaticspaces = 0;
1912 IntPtr sp;
1913
1914 for (int i = 0; i < ntopactivegeoms; i++)
3211 { 1915 {
3212 if (prim.IsPhysical && (d.BodyIsEnabled(prim.Body) || !prim._zeroFlag)) 1916 sp = d.SpaceGetGeom(ActiveSpace, i);
1917 if (d.GeomIsSpace(sp))
3213 { 1918 {
3214 prim.UpdatePositionAndVelocity(); 1919 nactivespaces++;
3215 1920 nactivegeoms += d.SpaceGetNumGeoms(sp);
3216 if (SupportsNINJAJoints)
3217 SimulateActorPendingJoints(prim);
3218 } 1921 }
1922 else
1923 nactivegeoms++;
3219 } 1924 }
3220 1925
3221 if (CollectStats) 1926 for (int i = 0; i < ntopstaticgeoms; i++)
3222 m_stats[ODEPrimUpdateFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick); 1927 {
1928 sp = d.SpaceGetGeom(StaticSpace, i);
1929 if (d.GeomIsSpace(sp))
1930 {
1931 nstaticspaces++;
1932 nstaticgeoms += d.SpaceGetNumGeoms(sp);
1933 }
1934 else
1935 nstaticgeoms++;
1936 }
3223 1937
3224 //DumpJointInfo(); 1938 int ntopgeoms = d.SpaceGetNumGeoms(TopSpace);
3225 1939
1940 int totgeoms = nstaticgeoms + nactivegeoms + ngroundgeoms + 1; // one ray
1941 int nbodies = d.NTotalBodies;
1942 int ngeoms = d.NTotalGeoms;
1943*/
3226 // Finished with all sim stepping. If requested, dump world state to file for debugging. 1944 // Finished with all sim stepping. If requested, dump world state to file for debugging.
3227 // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed? 1945 // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed?
3228 // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots? 1946 // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots?
@@ -3241,256 +1959,26 @@ namespace OpenSim.Region.Physics.OdePlugin
3241 1959
3242 d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix); 1960 d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix);
3243 } 1961 }
3244 1962
3245 latertickcount = Util.EnvironmentTickCountSubtract(tickCountFrameRun); 1963 // think time dilation as to do with dinamic step size that we dont' have
3246 1964 // even so tell something to world
3247 // OpenSimulator above does 10 fps. 10 fps = means that the main thread loop and physics 1965 if (nodeframes < 10) // we did the requested loops
3248 // has a max of 100 ms to run theoretically.
3249 // If the main loop stalls, it calls Simulate later which makes the tick count ms larger.
3250 // If Physics stalls, it takes longer which makes the tick count ms larger.
3251
3252 if (latertickcount < 100)
3253 {
3254 m_timeDilation = 1.0f; 1966 m_timeDilation = 1.0f;
3255 } 1967 else if (step_time > 0)
3256 else
3257 { 1968 {
3258 m_timeDilation = 100f / latertickcount; 1969 m_timeDilation = timeStep / step_time;
3259 //m_timeDilation = Math.Min((Math.Max(100 - (Util.EnvironmentTickCount() - tickCountFrameRun), 1) / 100f), 1.0f); 1970 if (m_timeDilation > 1)
1971 m_timeDilation = 1;
1972 if (step_time > m_SkipFramesAtms)
1973 step_time = 0;
3260 } 1974 }
3261
3262 tickCountFrameRun = Util.EnvironmentTickCount();
3263
3264 if (CollectStats)
3265 m_stats[ODETotalFrameMsStatName] += Util.EnvironmentTickCountSubtract(startFrameTick);
3266 } 1975 }
3267 1976
3268 return fps; 1977// return nodeframes * ODE_STEPSIZE; // return real simulated time
1978 return 1000 * nodeframes; // return steps for now * 1000 to keep core happy
3269 } 1979 }
3270 1980
3271 /// <summary> 1981 /// <summary>
3272 /// Simulate pending NINJA joints.
3273 /// </summary>
3274 /// <remarks>
3275 /// Called by the main Simulate() loop if NINJA joints are active. Should not be called from anywhere else.
3276 /// </remarks>
3277 private void SimulatePendingNINJAJoints()
3278 {
3279 // Create pending joints, if possible
3280
3281 // joints can only be processed after ALL bodies are processed (and exist in ODE), since creating
3282 // a joint requires specifying the body id of both involved bodies
3283 if (pendingJoints.Count > 0)
3284 {
3285 List<PhysicsJoint> successfullyProcessedPendingJoints = new List<PhysicsJoint>();
3286 //DoJointErrorMessage(joints_connecting_actor, "taint: " + pendingJoints.Count + " pending joints");
3287 foreach (PhysicsJoint joint in pendingJoints)
3288 {
3289 //DoJointErrorMessage(joint, "taint: time to create joint with parms: " + joint.RawParams);
3290 string[] jointParams = joint.RawParams.Split(" ".ToCharArray(), System.StringSplitOptions.RemoveEmptyEntries);
3291 List<IntPtr> jointBodies = new List<IntPtr>();
3292 bool allJointBodiesAreReady = true;
3293 foreach (string jointParam in jointParams)
3294 {
3295 if (jointParam == "NULL")
3296 {
3297 //DoJointErrorMessage(joint, "attaching NULL joint to world");
3298 jointBodies.Add(IntPtr.Zero);
3299 }
3300 else
3301 {
3302 //DoJointErrorMessage(joint, "looking for prim name: " + jointParam);
3303 bool foundPrim = false;
3304 lock (_prims)
3305 {
3306 foreach (OdePrim prim in _prims) // FIXME: inefficient
3307 {
3308 if (prim.SOPName == jointParam)
3309 {
3310 //DoJointErrorMessage(joint, "found for prim name: " + jointParam);
3311 if (prim.IsPhysical && prim.Body != IntPtr.Zero)
3312 {
3313 jointBodies.Add(prim.Body);
3314 foundPrim = true;
3315 break;
3316 }
3317 else
3318 {
3319 DoJointErrorMessage(joint, "prim name " + jointParam +
3320 " exists but is not (yet) physical; deferring joint creation. " +
3321 "IsPhysical property is " + prim.IsPhysical +
3322 " and body is " + prim.Body);
3323 foundPrim = false;
3324 break;
3325 }
3326 }
3327 }
3328 }
3329 if (foundPrim)
3330 {
3331 // all is fine
3332 }
3333 else
3334 {
3335 allJointBodiesAreReady = false;
3336 break;
3337 }
3338 }
3339 }
3340
3341 if (allJointBodiesAreReady)
3342 {
3343 //DoJointErrorMessage(joint, "allJointBodiesAreReady for " + joint.ObjectNameInScene + " with parms " + joint.RawParams);
3344 if (jointBodies[0] == jointBodies[1])
3345 {
3346 DoJointErrorMessage(joint, "ERROR: joint cannot be created; the joint bodies are the same, body1==body2. Raw body is " + jointBodies[0] + ". raw parms: " + joint.RawParams);
3347 }
3348 else
3349 {
3350 switch (joint.Type)
3351 {
3352 case PhysicsJointType.Ball:
3353 {
3354 IntPtr odeJoint;
3355 //DoJointErrorMessage(joint, "ODE creating ball joint ");
3356 odeJoint = d.JointCreateBall(world, IntPtr.Zero);
3357 //DoJointErrorMessage(joint, "ODE attaching ball joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
3358 d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
3359 //DoJointErrorMessage(joint, "ODE setting ball anchor: " + odeJoint + " to vec:" + joint.Position);
3360 d.JointSetBallAnchor(odeJoint,
3361 joint.Position.X,
3362 joint.Position.Y,
3363 joint.Position.Z);
3364 //DoJointErrorMessage(joint, "ODE joint setting OK");
3365 //DoJointErrorMessage(joint, "The ball joint's bodies are here: b0: ");
3366 //DoJointErrorMessage(joint, "" + (jointBodies[0] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[0]) : "fixed environment"));
3367 //DoJointErrorMessage(joint, "The ball joint's bodies are here: b1: ");
3368 //DoJointErrorMessage(joint, "" + (jointBodies[1] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[1]) : "fixed environment"));
3369
3370 if (joint is OdePhysicsJoint)
3371 {
3372 ((OdePhysicsJoint)joint).jointID = odeJoint;
3373 }
3374 else
3375 {
3376 DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
3377 }
3378 }
3379 break;
3380 case PhysicsJointType.Hinge:
3381 {
3382 IntPtr odeJoint;
3383 //DoJointErrorMessage(joint, "ODE creating hinge joint ");
3384 odeJoint = d.JointCreateHinge(world, IntPtr.Zero);
3385 //DoJointErrorMessage(joint, "ODE attaching hinge joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
3386 d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
3387 //DoJointErrorMessage(joint, "ODE setting hinge anchor: " + odeJoint + " to vec:" + joint.Position);
3388 d.JointSetHingeAnchor(odeJoint,
3389 joint.Position.X,
3390 joint.Position.Y,
3391 joint.Position.Z);
3392 // We use the orientation of the x-axis of the joint's coordinate frame
3393 // as the axis for the hinge.
3394
3395 // Therefore, we must get the joint's coordinate frame based on the
3396 // joint.Rotation field, which originates from the orientation of the
3397 // joint's proxy object in the scene.
3398
3399 // The joint's coordinate frame is defined as the transformation matrix
3400 // that converts a vector from joint-local coordinates into world coordinates.
3401 // World coordinates are defined as the XYZ coordinate system of the sim,
3402 // as shown in the top status-bar of the viewer.
3403
3404 // Once we have the joint's coordinate frame, we extract its X axis (AtAxis)
3405 // and use that as the hinge axis.
3406
3407 //joint.Rotation.Normalize();
3408 Matrix4 proxyFrame = Matrix4.CreateFromQuaternion(joint.Rotation);
3409
3410 // Now extract the X axis of the joint's coordinate frame.
3411
3412 // Do not try to use proxyFrame.AtAxis or you will become mired in the
3413 // tar pit of transposed, inverted, and generally messed-up orientations.
3414 // (In other words, Matrix4.AtAxis() is borked.)
3415 // Vector3 jointAxis = proxyFrame.AtAxis; <--- this path leadeth to madness
3416
3417 // Instead, compute the X axis of the coordinate frame by transforming
3418 // the (1,0,0) vector. At least that works.
3419
3420 //m_log.Debug("PHY: making axis: complete matrix is " + proxyFrame);
3421 Vector3 jointAxis = Vector3.Transform(Vector3.UnitX, proxyFrame);
3422 //m_log.Debug("PHY: making axis: hinge joint axis is " + jointAxis);
3423 //DoJointErrorMessage(joint, "ODE setting hinge axis: " + odeJoint + " to vec:" + jointAxis);
3424 d.JointSetHingeAxis(odeJoint,
3425 jointAxis.X,
3426 jointAxis.Y,
3427 jointAxis.Z);
3428 //d.JointSetHingeParam(odeJoint, (int)dParam.CFM, 0.1f);
3429 if (joint is OdePhysicsJoint)
3430 {
3431 ((OdePhysicsJoint)joint).jointID = odeJoint;
3432 }
3433 else
3434 {
3435 DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
3436 }
3437 }
3438 break;
3439 }
3440 successfullyProcessedPendingJoints.Add(joint);
3441 }
3442 }
3443 else
3444 {
3445 DoJointErrorMessage(joint, "joint could not yet be created; still pending");
3446 }
3447 }
3448
3449 foreach (PhysicsJoint successfullyProcessedJoint in successfullyProcessedPendingJoints)
3450 {
3451 //DoJointErrorMessage(successfullyProcessedJoint, "finalizing succesfully procsssed joint " + successfullyProcessedJoint.ObjectNameInScene + " parms " + successfullyProcessedJoint.RawParams);
3452 //DoJointErrorMessage(successfullyProcessedJoint, "removing from pending");
3453 InternalRemovePendingJoint(successfullyProcessedJoint);
3454 //DoJointErrorMessage(successfullyProcessedJoint, "adding to active");
3455 InternalAddActiveJoint(successfullyProcessedJoint);
3456 //DoJointErrorMessage(successfullyProcessedJoint, "done");
3457 }
3458 }
3459 }
3460
3461 /// <summary>
3462 /// Simulate the joint proxies of a NINJA actor.
3463 /// </summary>
3464 /// <remarks>
3465 /// Called as part of the Simulate() loop if NINJA physics is active. Must only be called from there.
3466 /// </remarks>
3467 /// <param name="actor"></param>
3468 private void SimulateActorPendingJoints(OdePrim actor)
3469 {
3470 // If an actor moved, move its joint proxy objects as well.
3471 // There seems to be an event PhysicsActor.OnPositionUpdate that could be used
3472 // for this purpose but it is never called! So we just do the joint
3473 // movement code here.
3474
3475 if (actor.SOPName != null &&
3476 joints_connecting_actor.ContainsKey(actor.SOPName) &&
3477 joints_connecting_actor[actor.SOPName] != null &&
3478 joints_connecting_actor[actor.SOPName].Count > 0)
3479 {
3480 foreach (PhysicsJoint affectedJoint in joints_connecting_actor[actor.SOPName])
3481 {
3482 if (affectedJoint.IsInPhysicsEngine)
3483 {
3484 DoJointMoved(affectedJoint);
3485 }
3486 else
3487 {
3488 DoJointErrorMessage(affectedJoint, "a body connected to a joint was moved, but the joint doesn't exist yet! this will lead to joint error. joint was: " + affectedJoint.ObjectNameInScene + " parms:" + affectedJoint.RawParams);
3489 }
3490 }
3491 }
3492 }
3493
3494 public override void GetResults() 1982 public override void GetResults()
3495 { 1983 {
3496 } 1984 }
@@ -3498,275 +1986,141 @@ namespace OpenSim.Region.Physics.OdePlugin
3498 public override bool IsThreaded 1986 public override bool IsThreaded
3499 { 1987 {
3500 // for now we won't be multithreaded 1988 // for now we won't be multithreaded
3501 get { return false; } 1989 get { return (false); }
3502 } 1990 }
3503 1991
3504 #region ODE Specific Terrain Fixes 1992 public float GetTerrainHeightAtXY(float x, float y)
3505 private float[] ResizeTerrain512NearestNeighbour(float[] heightMap)
3506 { 1993 {
3507 float[] returnarr = new float[262144];
3508 float[,] resultarr = new float[(int)WorldExtents.X, (int)WorldExtents.Y];
3509 1994
3510 // Filling out the array into its multi-dimensional components
3511 for (int y = 0; y < WorldExtents.Y; y++)
3512 {
3513 for (int x = 0; x < WorldExtents.X; x++)
3514 {
3515 resultarr[y, x] = heightMap[y * (int)WorldExtents.Y + x];
3516 }
3517 }
3518 1995
3519 // Resize using Nearest Neighbour 1996 int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
3520 1997 int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
3521 // This particular way is quick but it only works on a multiple of the original
3522
3523 // The idea behind this method can be described with the following diagrams
3524 // second pass and third pass happen in the same loop really.. just separated
3525 // them to show what this does.
3526
3527 // First Pass
3528 // ResultArr:
3529 // 1,1,1,1,1,1
3530 // 1,1,1,1,1,1
3531 // 1,1,1,1,1,1
3532 // 1,1,1,1,1,1
3533 // 1,1,1,1,1,1
3534 // 1,1,1,1,1,1
3535
3536 // Second Pass
3537 // ResultArr2:
3538 // 1,,1,,1,,1,,1,,1,
3539 // ,,,,,,,,,,
3540 // 1,,1,,1,,1,,1,,1,
3541 // ,,,,,,,,,,
3542 // 1,,1,,1,,1,,1,,1,
3543 // ,,,,,,,,,,
3544 // 1,,1,,1,,1,,1,,1,
3545 // ,,,,,,,,,,
3546 // 1,,1,,1,,1,,1,,1,
3547 // ,,,,,,,,,,
3548 // 1,,1,,1,,1,,1,,1,
3549
3550 // Third pass fills in the blanks
3551 // ResultArr2:
3552 // 1,1,1,1,1,1,1,1,1,1,1,1
3553 // 1,1,1,1,1,1,1,1,1,1,1,1
3554 // 1,1,1,1,1,1,1,1,1,1,1,1
3555 // 1,1,1,1,1,1,1,1,1,1,1,1
3556 // 1,1,1,1,1,1,1,1,1,1,1,1
3557 // 1,1,1,1,1,1,1,1,1,1,1,1
3558 // 1,1,1,1,1,1,1,1,1,1,1,1
3559 // 1,1,1,1,1,1,1,1,1,1,1,1
3560 // 1,1,1,1,1,1,1,1,1,1,1,1
3561 // 1,1,1,1,1,1,1,1,1,1,1,1
3562 // 1,1,1,1,1,1,1,1,1,1,1,1
3563
3564 // X,Y = .
3565 // X+1,y = ^
3566 // X,Y+1 = *
3567 // X+1,Y+1 = #
3568
3569 // Filling in like this;
3570 // .*
3571 // ^#
3572 // 1st .
3573 // 2nd *
3574 // 3rd ^
3575 // 4th #
3576 // on single loop.
3577
3578 float[,] resultarr2 = new float[512, 512];
3579 for (int y = 0; y < WorldExtents.Y; y++)
3580 {
3581 for (int x = 0; x < WorldExtents.X; x++)
3582 {
3583 resultarr2[y * 2, x * 2] = resultarr[y, x];
3584 1998
3585 if (y < WorldExtents.Y)
3586 {
3587 resultarr2[(y * 2) + 1, x * 2] = resultarr[y, x];
3588 }
3589 if (x < WorldExtents.X)
3590 {
3591 resultarr2[y * 2, (x * 2) + 1] = resultarr[y, x];
3592 }
3593 if (x < WorldExtents.X && y < WorldExtents.Y)
3594 {
3595 resultarr2[(y * 2) + 1, (x * 2) + 1] = resultarr[y, x];
3596 }
3597 }
3598 }
3599 1999
3600 //Flatten out the array 2000 IntPtr heightFieldGeom = IntPtr.Zero;
3601 int i = 0;
3602 for (int y = 0; y < 512; y++)
3603 {
3604 for (int x = 0; x < 512; x++)
3605 {
3606 if (resultarr2[y, x] <= 0)
3607 returnarr[i] = 0.0000001f;
3608 else
3609 returnarr[i] = resultarr2[y, x];
3610 2001
3611 i++; 2002 // get region map
3612 } 2003 if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom))
3613 } 2004 return 0f;
3614 2005
3615 return returnarr; 2006 if (heightFieldGeom == IntPtr.Zero)
3616 } 2007 return 0f;
3617 2008
3618 private float[] ResizeTerrain512Interpolation(float[] heightMap) 2009 if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom))
3619 { 2010 return 0f;
3620 float[] returnarr = new float[262144]; 2011
3621 float[,] resultarr = new float[512,512]; 2012 // TerrainHeightField for ODE as offset 1m
2013 x += 1f - offsetX;
2014 y += 1f - offsetY;
2015
2016 // make position fit into array
2017 if (x < 0)
2018 x = 0;
2019 if (y < 0)
2020 y = 0;
3622 2021
3623 // Filling out the array into its multi-dimensional components 2022 // integer indexs
3624 for (int y = 0; y < 256; y++) 2023 int ix;
2024 int iy;
2025 // interpolators offset
2026 float dx;
2027 float dy;
2028
2029 int regsize = (int)Constants.RegionSize + 3; // map size see setterrain number of samples
2030
2031 if (OdeUbitLib)
3625 { 2032 {
3626 for (int x = 0; x < 256; x++) 2033 if (x < regsize - 1)
2034 {
2035 ix = (int)x;
2036 dx = x - (float)ix;
2037 }
2038 else // out world use external height
2039 {
2040 ix = regsize - 2;
2041 dx = 0;
2042 }
2043 if (y < regsize - 1)
3627 { 2044 {
3628 resultarr[y, x] = heightMap[y * 256 + x]; 2045 iy = (int)y;
2046 dy = y - (float)iy;
2047 }
2048 else
2049 {
2050 iy = regsize - 2;
2051 dy = 0;
3629 } 2052 }
3630 } 2053 }
3631 2054
3632 // Resize using interpolation 2055 else
3633
3634 // This particular way is quick but it only works on a multiple of the original
3635
3636 // The idea behind this method can be described with the following diagrams
3637 // second pass and third pass happen in the same loop really.. just separated
3638 // them to show what this does.
3639
3640 // First Pass
3641 // ResultArr:
3642 // 1,1,1,1,1,1
3643 // 1,1,1,1,1,1
3644 // 1,1,1,1,1,1
3645 // 1,1,1,1,1,1
3646 // 1,1,1,1,1,1
3647 // 1,1,1,1,1,1
3648
3649 // Second Pass
3650 // ResultArr2:
3651 // 1,,1,,1,,1,,1,,1,
3652 // ,,,,,,,,,,
3653 // 1,,1,,1,,1,,1,,1,
3654 // ,,,,,,,,,,
3655 // 1,,1,,1,,1,,1,,1,
3656 // ,,,,,,,,,,
3657 // 1,,1,,1,,1,,1,,1,
3658 // ,,,,,,,,,,
3659 // 1,,1,,1,,1,,1,,1,
3660 // ,,,,,,,,,,
3661 // 1,,1,,1,,1,,1,,1,
3662
3663 // Third pass fills in the blanks
3664 // ResultArr2:
3665 // 1,1,1,1,1,1,1,1,1,1,1,1
3666 // 1,1,1,1,1,1,1,1,1,1,1,1
3667 // 1,1,1,1,1,1,1,1,1,1,1,1
3668 // 1,1,1,1,1,1,1,1,1,1,1,1
3669 // 1,1,1,1,1,1,1,1,1,1,1,1
3670 // 1,1,1,1,1,1,1,1,1,1,1,1
3671 // 1,1,1,1,1,1,1,1,1,1,1,1
3672 // 1,1,1,1,1,1,1,1,1,1,1,1
3673 // 1,1,1,1,1,1,1,1,1,1,1,1
3674 // 1,1,1,1,1,1,1,1,1,1,1,1
3675 // 1,1,1,1,1,1,1,1,1,1,1,1
3676
3677 // X,Y = .
3678 // X+1,y = ^
3679 // X,Y+1 = *
3680 // X+1,Y+1 = #
3681
3682 // Filling in like this;
3683 // .*
3684 // ^#
3685 // 1st .
3686 // 2nd *
3687 // 3rd ^
3688 // 4th #
3689 // on single loop.
3690
3691 float[,] resultarr2 = new float[512,512];
3692 for (int y = 0; y < (int)Constants.RegionSize; y++)
3693 { 2056 {
3694 for (int x = 0; x < (int)Constants.RegionSize; x++) 2057 // we still have square fixed size regions
2058 // also flip x and y because of how map is done for ODE fliped axis
2059 // so ix,iy,dx and dy are inter exchanged
2060 if (x < regsize - 1)
2061 {
2062 iy = (int)x;
2063 dy = x - (float)iy;
2064 }
2065 else // out world use external height
2066 {
2067 iy = regsize - 2;
2068 dy = 0;
2069 }
2070 if (y < regsize - 1)
2071 {
2072 ix = (int)y;
2073 dx = y - (float)ix;
2074 }
2075 else
3695 { 2076 {
3696 resultarr2[y*2, x*2] = resultarr[y, x]; 2077 ix = regsize - 2;
2078 dx = 0;
2079 }
2080 }
3697 2081
3698 if (y < (int)Constants.RegionSize) 2082 float h0;
3699 { 2083 float h1;
3700 if (y + 1 < (int)Constants.RegionSize) 2084 float h2;
3701 { 2085
3702 if (x + 1 < (int)Constants.RegionSize) 2086 iy *= regsize;
3703 { 2087 iy += ix; // all indexes have iy + ix
3704 resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x] + 2088
3705 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4); 2089 float[] heights = TerrainHeightFieldHeights[heightFieldGeom];
3706 } 2090 /*
3707 else 2091 if ((dx + dy) <= 1.0f)
3708 {
3709 resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x])/2);
3710 }
3711 }
3712 else
3713 {
3714 resultarr2[(y*2) + 1, x*2] = resultarr[y, x];
3715 }
3716 }
3717 if (x < (int)Constants.RegionSize)
3718 {
3719 if (x + 1 < (int)Constants.RegionSize)
3720 {
3721 if (y + 1 < (int)Constants.RegionSize)
3722 {
3723 resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
3724 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
3725 }
3726 else
3727 {
3728 resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y, x + 1])/2);
3729 }
3730 }
3731 else
3732 {
3733 resultarr2[y*2, (x*2) + 1] = resultarr[y, x];
3734 }
3735 }
3736 if (x < (int)Constants.RegionSize && y < (int)Constants.RegionSize)
3737 {
3738 if ((x + 1 < (int)Constants.RegionSize) && (y + 1 < (int)Constants.RegionSize))
3739 { 2092 {
3740 resultarr2[(y*2) + 1, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] + 2093 h0 = ((float)heights[iy]); // 0,0 vertice
3741 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4); 2094 h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0
2095 h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0
3742 } 2096 }
3743 else 2097 else
3744 { 2098 {
3745 resultarr2[(y*2) + 1, (x*2) + 1] = resultarr[y, x]; 2099 h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice
2100 h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0
2101 h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1
3746 } 2102 }
3747 } 2103 */
3748 } 2104 h0 = ((float)heights[iy]); // 0,0 vertice
2105
2106 if ((dy > dx))
2107 {
2108 iy += regsize;
2109 h2 = (float)heights[iy]; // 0,1 vertice
2110 h1 = (h2 - h0) * dy; // 0,1 vertice minus 0,0
2111 h2 = ((float)heights[iy + 1] - h2) * dx; // 1,1 vertice minus 0,1
3749 } 2112 }
3750 //Flatten out the array 2113 else
3751 int i = 0;
3752 for (int y = 0; y < 512; y++)
3753 { 2114 {
3754 for (int x = 0; x < 512; x++) 2115 iy++;
3755 { 2116 h2 = (float)heights[iy]; // vertice 1,0
3756 if (Single.IsNaN(resultarr2[y, x]) || Single.IsInfinity(resultarr2[y, x])) 2117 h1 = (h2 - h0) * dx; // 1,0 vertice minus 0,0
3757 { 2118 h2 = (((float)heights[iy + regsize]) - h2) * dy; // 1,1 vertice minus 1,0
3758 m_log.Warn("[ODE SCENE]: Non finite heightfield element detected. Setting it to 0");
3759 resultarr2[y, x] = 0;
3760 }
3761 returnarr[i] = resultarr2[y, x];
3762 i++;
3763 }
3764 } 2119 }
3765 2120
3766 return returnarr; 2121 return h0 + h1 + h2;
3767 } 2122 }
3768 2123
3769 #endregion
3770 2124
3771 public override void SetTerrain(float[] heightMap) 2125 public override void SetTerrain(float[] heightMap)
3772 { 2126 {
@@ -3783,78 +2137,75 @@ namespace OpenSim.Region.Physics.OdePlugin
3783 } 2137 }
3784 } 2138 }
3785 2139
3786 private void SetTerrain(float[] heightMap, Vector3 pOffset) 2140 public override void CombineTerrain(float[] heightMap, Vector3 pOffset)
3787 { 2141 {
3788 int startTime = Util.EnvironmentTickCount(); 2142 SetTerrain(heightMap, pOffset);
3789 m_log.DebugFormat("[ODE SCENE]: Setting terrain for {0} with offset {1}", Name, pOffset); 2143 }
3790
3791 // this._heightmap[i] = (double)heightMap[i];
3792 // dbm (danx0r) -- creating a buffer zone of one extra sample all around
3793 //_origheightmap = heightMap;
3794
3795 float[] _heightmap;
3796
3797 // zero out a heightmap array float array (single dimension [flattened]))
3798 //if ((int)Constants.RegionSize == 256)
3799 // _heightmap = new float[514 * 514];
3800 //else
3801 2144
3802 _heightmap = new float[(((int)Constants.RegionSize + 2) * ((int)Constants.RegionSize + 2))]; 2145 public void SetTerrain(float[] heightMap, Vector3 pOffset)
2146 {
2147 if (OdeUbitLib)
2148 UbitSetTerrain(heightMap, pOffset);
2149 else
2150 OriSetTerrain(heightMap, pOffset);
2151 }
3803 2152
3804 uint heightmapWidth = Constants.RegionSize + 1; 2153 public void OriSetTerrain(float[] heightMap, Vector3 pOffset)
3805 uint heightmapHeight = Constants.RegionSize + 1; 2154 {
2155 // assumes 1m size grid and constante size square regions
2156 // needs to know about sims around in future
3806 2157
3807 uint heightmapWidthSamples; 2158 float[] _heightmap;
3808 2159
3809 uint heightmapHeightSamples; 2160 uint heightmapWidth = Constants.RegionSize + 2;
2161 uint heightmapHeight = Constants.RegionSize + 2;
3810 2162
3811 //if (((int)Constants.RegionSize) == 256) 2163 uint heightmapWidthSamples = heightmapWidth + 1;
3812 //{ 2164 uint heightmapHeightSamples = heightmapHeight + 1;
3813 // heightmapWidthSamples = 2 * (uint)Constants.RegionSize + 2;
3814 // heightmapHeightSamples = 2 * (uint)Constants.RegionSize + 2;
3815 // heightmapWidth++;
3816 // heightmapHeight++;
3817 //}
3818 //else
3819 //{
3820 2165
3821 heightmapWidthSamples = (uint)Constants.RegionSize + 1; 2166 _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
3822 heightmapHeightSamples = (uint)Constants.RegionSize + 1;
3823 //}
3824 2167
3825 const float scale = 1.0f; 2168 const float scale = 1.0f;
3826 const float offset = 0.0f; 2169 const float offset = 0.0f;
3827 const float thickness = 0.2f; 2170 const float thickness = 10f;
3828 const int wrap = 0; 2171 const int wrap = 0;
3829 2172
3830 int regionsize = (int) Constants.RegionSize + 2; 2173 uint regionsize = Constants.RegionSize;
3831 //Double resolution 2174
3832 //if (((int)Constants.RegionSize) == 256) 2175 float hfmin = float.MaxValue;
3833 // heightMap = ResizeTerrain512Interpolation(heightMap); 2176 float hfmax = float.MinValue;
2177 float val;
2178 uint xx;
2179 uint yy;
3834 2180
2181 uint maxXXYY = regionsize - 1;
2182 // flipping map adding one margin all around so things don't fall in edges
3835 2183
3836 // if (((int)Constants.RegionSize) == 256 && (int)Constants.RegionSize == 256) 2184 uint xt = 0;
3837 // regionsize = 512; 2185 xx = 0;
3838 2186
3839 float hfmin = 2000; 2187 for (uint x = 0; x < heightmapWidthSamples; x++)
3840 float hfmax = -2000;
3841
3842 for (int x = 0; x < heightmapWidthSamples; x++)
3843 { 2188 {
3844 for (int y = 0; y < heightmapHeightSamples; y++) 2189 if (x > 1 && xx < maxXXYY)
2190 xx++;
2191 yy = 0;
2192 for (uint y = 0; y < heightmapHeightSamples; y++)
3845 { 2193 {
3846 int xx = Util.Clip(x - 1, 0, regionsize - 1); 2194 if (y > 1 && y < maxXXYY)
3847 int yy = Util.Clip(y - 1, 0, regionsize - 1); 2195 yy += regionsize;
3848 2196
3849 2197 val = heightMap[yy + xx];
3850 float val= heightMap[yy * (int)Constants.RegionSize + xx]; 2198 if (val < 0.0f)
3851 _heightmap[x * ((int)Constants.RegionSize + 2) + y] = val; 2199 val = 0.0f; // no neg terrain as in chode
3852 2200 _heightmap[xt + y] = val;
3853 hfmin = (val < hfmin) ? val : hfmin; 2201
3854 hfmax = (val > hfmax) ? val : hfmax; 2202 if (hfmin > val)
2203 hfmin = val;
2204 if (hfmax < val)
2205 hfmax = val;
3855 } 2206 }
2207 xt += heightmapHeightSamples;
3856 } 2208 }
3857
3858 lock (OdeLock) 2209 lock (OdeLock)
3859 { 2210 {
3860 IntPtr GroundGeom = IntPtr.Zero; 2211 IntPtr GroundGeom = IntPtr.Zero;
@@ -3863,62 +2214,177 @@ namespace OpenSim.Region.Physics.OdePlugin
3863 RegionTerrain.Remove(pOffset); 2214 RegionTerrain.Remove(pOffset);
3864 if (GroundGeom != IntPtr.Zero) 2215 if (GroundGeom != IntPtr.Zero)
3865 { 2216 {
2217 actor_name_map.Remove(GroundGeom);
2218 d.GeomDestroy(GroundGeom);
2219
3866 if (TerrainHeightFieldHeights.ContainsKey(GroundGeom)) 2220 if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
3867 { 2221 {
2222 TerrainHeightFieldHeightsHandlers[GroundGeom].Free();
2223 TerrainHeightFieldHeightsHandlers.Remove(GroundGeom);
3868 TerrainHeightFieldHeights.Remove(GroundGeom); 2224 TerrainHeightFieldHeights.Remove(GroundGeom);
3869 } 2225 }
3870 d.SpaceRemove(space, GroundGeom);
3871 d.GeomDestroy(GroundGeom);
3872 } 2226 }
3873
3874 } 2227 }
3875 IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); 2228 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
3876 d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmap, 0, heightmapWidth + 1, heightmapHeight + 1, 2229
3877 (int)heightmapWidthSamples + 1, (int)heightmapHeightSamples + 1, scale, 2230 GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);
2231
2232 d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, heightmapWidth , heightmapHeight,
2233 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
3878 offset, thickness, wrap); 2234 offset, thickness, wrap);
2235
3879 d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); 2236 d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
3880 GroundGeom = d.CreateHeightfield(space, HeightmapData, 1); 2237
2238 GroundGeom = d.CreateHeightfield(GroundSpace, HeightmapData, 1);
2239
3881 if (GroundGeom != IntPtr.Zero) 2240 if (GroundGeom != IntPtr.Zero)
3882 { 2241 {
3883 d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land)); 2242 d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land));
3884 d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space)); 2243 d.GeomSetCollideBits(GroundGeom, 0);
2244
2245 PhysicsActor pa = new NullPhysicsActor();
2246 pa.Name = "Terrain";
2247 pa.PhysicsActorType = (int)ActorTypes.Ground;
2248 actor_name_map[GroundGeom] = pa;
2249
2250// geom_name_map[GroundGeom] = "Terrain";
2251
2252 d.Matrix3 R = new d.Matrix3();
2253
2254 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
2255 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
3885 2256
2257
2258 q1 = q1 * q2;
2259
2260 Vector3 v3;
2261 float angle;
2262 q1.GetAxisAngle(out v3, out angle);
2263
2264 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
2265 d.GeomSetRotation(GroundGeom, ref R);
2266 d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0);
2267 RegionTerrain.Add(pOffset, GroundGeom);
2268 TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);
2269 TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler);
3886 } 2270 }
3887 geom_name_map[GroundGeom] = "Terrain"; 2271 }
2272 }
3888 2273
3889 d.Matrix3 R = new d.Matrix3(); 2274 public void UbitSetTerrain(float[] heightMap, Vector3 pOffset)
2275 {
2276 // assumes 1m size grid and constante size square regions
2277 // needs to know about sims around in future
3890 2278
3891 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); 2279 float[] _heightmap;
3892 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); 2280
3893 //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1)); 2281 uint heightmapWidth = Constants.RegionSize + 2;
2282 uint heightmapHeight = Constants.RegionSize + 2;
2283
2284 uint heightmapWidthSamples = heightmapWidth + 1;
2285 uint heightmapHeightSamples = heightmapHeight + 1;
2286
2287 _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
2288
2289
2290 uint regionsize = Constants.RegionSize;
3894 2291
3895 q1 = q1 * q2; 2292 float hfmin = float.MaxValue;
3896 //q1 = q1 * q3; 2293// float hfmax = float.MinValue;
3897 Vector3 v3; 2294 float val;
3898 float angle;
3899 q1.GetAxisAngle(out v3, out angle);
3900 2295
3901 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); 2296
3902 d.GeomSetRotation(GroundGeom, ref R); 2297 uint maxXXYY = regionsize - 1;
3903 d.GeomSetPosition(GroundGeom, (pOffset.X + ((int)Constants.RegionSize * 0.5f)), (pOffset.Y + ((int)Constants.RegionSize * 0.5f)), 0); 2298 // adding one margin all around so things don't fall in edges
3904 IntPtr testGround = IntPtr.Zero; 2299
3905 if (RegionTerrain.TryGetValue(pOffset, out testGround)) 2300 uint xx;
2301 uint yy = 0;
2302 uint yt = 0;
2303
2304 for (uint y = 0; y < heightmapHeightSamples; y++)
2305 {
2306 if (y > 1 && y < maxXXYY)
2307 yy += regionsize;
2308 xx = 0;
2309 for (uint x = 0; x < heightmapWidthSamples; x++)
3906 { 2310 {
3907 RegionTerrain.Remove(pOffset); 2311 if (x > 1 && x < maxXXYY)
2312 xx++;
2313
2314 val = heightMap[yy + xx];
2315 if (val < 0.0f)
2316 val = 0.0f; // no neg terrain as in chode
2317 _heightmap[yt + x] = val;
2318
2319 if (hfmin > val)
2320 hfmin = val;
2321// if (hfmax < val)
2322// hfmax = val;
3908 } 2323 }
3909 RegionTerrain.Add(pOffset, GroundGeom, GroundGeom); 2324 yt += heightmapWidthSamples;
3910 TerrainHeightFieldHeights.Add(GroundGeom,_heightmap);
3911 } 2325 }
2326 lock (OdeLock)
2327 {
2328 IntPtr GroundGeom = IntPtr.Zero;
2329 if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
2330 {
2331 RegionTerrain.Remove(pOffset);
2332 if (GroundGeom != IntPtr.Zero)
2333 {
2334 actor_name_map.Remove(GroundGeom);
2335 d.GeomDestroy(GroundGeom);
2336
2337 if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
2338 {
2339 if (TerrainHeightFieldHeightsHandlers[GroundGeom].IsAllocated)
2340 TerrainHeightFieldHeightsHandlers[GroundGeom].Free();
2341 TerrainHeightFieldHeightsHandlers.Remove(GroundGeom);
2342 TerrainHeightFieldHeights.Remove(GroundGeom);
2343 }
2344 }
2345 }
2346 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
2347
2348 const int wrap = 0;
2349 float thickness = hfmin;
2350 if (thickness < 0)
2351 thickness = 1;
2352
2353 GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);
3912 2354
3913 m_log.DebugFormat( 2355 d.GeomUbitTerrainDataBuild(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, 1.0f,
3914 "[ODE SCENE]: Setting terrain for {0} took {1}ms", Name, Util.EnvironmentTickCountSubtract(startTime)); 2356 (int)heightmapWidthSamples, (int)heightmapHeightSamples,
2357 thickness, wrap);
2358
2359// d.GeomUbitTerrainDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
2360 GroundGeom = d.CreateUbitTerrain(GroundSpace, HeightmapData, 1);
2361 if (GroundGeom != IntPtr.Zero)
2362 {
2363 d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land));
2364 d.GeomSetCollideBits(GroundGeom, 0);
2365
2366
2367 PhysicsActor pa = new NullPhysicsActor();
2368 pa.Name = "Terrain";
2369 pa.PhysicsActorType = (int)ActorTypes.Ground;
2370 actor_name_map[GroundGeom] = pa;
2371
2372// geom_name_map[GroundGeom] = "Terrain";
2373
2374 d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0);
2375 RegionTerrain.Add(pOffset, GroundGeom);
2376 TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);
2377 TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler);
2378 }
2379 }
3915 } 2380 }
3916 2381
2382
3917 public override void DeleteTerrain() 2383 public override void DeleteTerrain()
3918 { 2384 {
3919 } 2385 }
3920 2386
3921 internal float GetWaterLevel() 2387 public float GetWaterLevel()
3922 { 2388 {
3923 return waterlevel; 2389 return waterlevel;
3924 } 2390 }
@@ -3927,169 +2393,252 @@ namespace OpenSim.Region.Physics.OdePlugin
3927 { 2393 {
3928 return true; 2394 return true;
3929 } 2395 }
2396/*
2397 public override void UnCombine(PhysicsScene pScene)
2398 {
2399 IntPtr localGround = IntPtr.Zero;
2400// float[] localHeightfield;
2401 bool proceed = false;
2402 List<IntPtr> geomDestroyList = new List<IntPtr>();
3930 2403
3931// public override void UnCombine(PhysicsScene pScene) 2404 lock (OdeLock)
3932// { 2405 {
3933// IntPtr localGround = IntPtr.Zero; 2406 if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround))
3934//// float[] localHeightfield; 2407 {
3935// bool proceed = false; 2408 foreach (IntPtr geom in TerrainHeightFieldHeights.Keys)
3936// List<IntPtr> geomDestroyList = new List<IntPtr>(); 2409 {
3937// 2410 if (geom == localGround)
3938// lock (OdeLock) 2411 {
3939// { 2412// localHeightfield = TerrainHeightFieldHeights[geom];
3940// if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround)) 2413 proceed = true;
3941// { 2414 }
3942// foreach (IntPtr geom in TerrainHeightFieldHeights.Keys) 2415 else
3943// { 2416 {
3944// if (geom == localGround) 2417 geomDestroyList.Add(geom);
3945// { 2418 }
3946//// localHeightfield = TerrainHeightFieldHeights[geom]; 2419 }
3947// proceed = true; 2420
3948// } 2421 if (proceed)
3949// else 2422 {
3950// { 2423 m_worldOffset = Vector3.Zero;
3951// geomDestroyList.Add(geom); 2424 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
3952// } 2425 m_parentScene = null;
3953// } 2426
3954// 2427 foreach (IntPtr g in geomDestroyList)
3955// if (proceed) 2428 {
3956// { 2429 // removingHeightField needs to be done or the garbage collector will
3957// m_worldOffset = Vector3.Zero; 2430 // collect the terrain data before we tell ODE to destroy it causing
3958// WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); 2431 // memory corruption
3959// m_parentScene = null; 2432 if (TerrainHeightFieldHeights.ContainsKey(g))
3960// 2433 {
3961// foreach (IntPtr g in geomDestroyList) 2434// float[] removingHeightField = TerrainHeightFieldHeights[g];
3962// { 2435 TerrainHeightFieldHeights.Remove(g);
3963// // removingHeightField needs to be done or the garbage collector will 2436
3964// // collect the terrain data before we tell ODE to destroy it causing 2437 if (RegionTerrain.ContainsKey(g))
3965// // memory corruption 2438 {
3966// if (TerrainHeightFieldHeights.ContainsKey(g)) 2439 RegionTerrain.Remove(g);
3967// { 2440 }
3968//// float[] removingHeightField = TerrainHeightFieldHeights[g]; 2441
3969// TerrainHeightFieldHeights.Remove(g); 2442 d.GeomDestroy(g);
3970// 2443 //removingHeightField = new float[0];
3971// if (RegionTerrain.ContainsKey(g)) 2444 }
3972// { 2445 }
3973// RegionTerrain.Remove(g);
3974// }
3975//
3976// d.GeomDestroy(g);
3977// //removingHeightField = new float[0];
3978// }
3979// }
3980//
3981// }
3982// else
3983// {
3984// m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data.");
3985// }
3986// }
3987// }
3988// }
3989 2446
2447 }
2448 else
2449 {
2450 m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data.");
2451 }
2452 }
2453 }
2454 }
2455*/
3990 public override void SetWaterLevel(float baseheight) 2456 public override void SetWaterLevel(float baseheight)
3991 { 2457 {
3992 waterlevel = baseheight; 2458 waterlevel = baseheight;
3993 randomizeWater(waterlevel); 2459// randomizeWater(waterlevel);
3994 } 2460 }
3995 2461/*
3996 private void randomizeWater(float baseheight) 2462 public void randomizeWater(float baseheight)
3997 { 2463 {
3998 const uint heightmapWidth = m_regionWidth + 2; 2464 const uint heightmapWidth = Constants.RegionSize + 2;
3999 const uint heightmapHeight = m_regionHeight + 2; 2465 const uint heightmapHeight = Constants.RegionSize + 2;
4000 const uint heightmapWidthSamples = m_regionWidth + 2; 2466 const uint heightmapWidthSamples = heightmapWidth + 1;
4001 const uint heightmapHeightSamples = m_regionHeight + 2; 2467 const uint heightmapHeightSamples = heightmapHeight + 1;
2468
4002 const float scale = 1.0f; 2469 const float scale = 1.0f;
4003 const float offset = 0.0f; 2470 const float offset = 0.0f;
4004 const float thickness = 2.9f;
4005 const int wrap = 0; 2471 const int wrap = 0;
4006 2472
4007 for (int i = 0; i < (258 * 258); i++) 2473 float[] _watermap = new float[heightmapWidthSamples * heightmapWidthSamples];
2474
2475 float maxheigh = float.MinValue;
2476 float minheigh = float.MaxValue;
2477 float val;
2478 for (int i = 0; i < (heightmapWidthSamples * heightmapHeightSamples); i++)
4008 { 2479 {
4009 _watermap[i] = (baseheight-0.1f) + ((float)fluidRandomizer.Next(1,9) / 10f); 2480
4010 // m_log.Info((baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f)); 2481 val = (baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f);
2482 _watermap[i] = val;
2483 if (maxheigh < val)
2484 maxheigh = val;
2485 if (minheigh > val)
2486 minheigh = val;
4011 } 2487 }
4012 2488
2489 float thickness = minheigh;
2490
4013 lock (OdeLock) 2491 lock (OdeLock)
4014 { 2492 {
4015 if (WaterGeom != IntPtr.Zero) 2493 if (WaterGeom != IntPtr.Zero)
4016 { 2494 {
4017 d.SpaceRemove(space, WaterGeom); 2495 actor_name_map.Remove(WaterGeom);
2496 d.GeomDestroy(WaterGeom);
2497 d.GeomHeightfieldDataDestroy(WaterHeightmapData);
2498 WaterGeom = IntPtr.Zero;
2499 WaterHeightmapData = IntPtr.Zero;
2500 if(WaterMapHandler.IsAllocated)
2501 WaterMapHandler.Free();
4018 } 2502 }
4019 IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); 2503
4020 d.GeomHeightfieldDataBuildSingle(HeightmapData, _watermap, 0, heightmapWidth, heightmapHeight, 2504 WaterHeightmapData = d.GeomHeightfieldDataCreate();
2505
2506 WaterMapHandler = GCHandle.Alloc(_watermap, GCHandleType.Pinned);
2507
2508 d.GeomHeightfieldDataBuildSingle(WaterHeightmapData, WaterMapHandler.AddrOfPinnedObject(), 0, heightmapWidth, heightmapHeight,
4021 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale, 2509 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
4022 offset, thickness, wrap); 2510 offset, thickness, wrap);
4023 d.GeomHeightfieldDataSetBounds(HeightmapData, m_regionWidth, m_regionHeight); 2511 d.GeomHeightfieldDataSetBounds(WaterHeightmapData, minheigh, maxheigh);
4024 WaterGeom = d.CreateHeightfield(space, HeightmapData, 1); 2512 WaterGeom = d.CreateHeightfield(StaticSpace, WaterHeightmapData, 1);
4025 if (WaterGeom != IntPtr.Zero) 2513 if (WaterGeom != IntPtr.Zero)
4026 { 2514 {
4027 d.GeomSetCategoryBits(WaterGeom, (int)(CollisionCategories.Water)); 2515 d.GeomSetCategoryBits(WaterGeom, (uint)(CollisionCategories.Water));
4028 d.GeomSetCollideBits(WaterGeom, (int)(CollisionCategories.Space)); 2516 d.GeomSetCollideBits(WaterGeom, 0);
4029 } 2517
4030 2518
4031 geom_name_map[WaterGeom] = "Water"; 2519 PhysicsActor pa = new NullPhysicsActor();
2520 pa.Name = "Water";
2521 pa.PhysicsActorType = (int)ActorTypes.Water;
4032 2522
4033 d.Matrix3 R = new d.Matrix3(); 2523 actor_name_map[WaterGeom] = pa;
2524// geom_name_map[WaterGeom] = "Water";
4034 2525
4035 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); 2526 d.Matrix3 R = new d.Matrix3();
4036 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
4037 //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1));
4038 2527
4039 q1 = q1 * q2; 2528 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
4040 //q1 = q1 * q3; 2529 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
4041 Vector3 v3;
4042 float angle;
4043 q1.GetAxisAngle(out v3, out angle);
4044 2530
4045 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); 2531 q1 = q1 * q2;
4046 d.GeomSetRotation(WaterGeom, ref R); 2532 Vector3 v3;
4047 d.GeomSetPosition(WaterGeom, 128, 128, 0); 2533 float angle;
2534 q1.GetAxisAngle(out v3, out angle);
2535
2536 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
2537 d.GeomSetRotation(WaterGeom, ref R);
2538 d.GeomSetPosition(WaterGeom, (float)Constants.RegionSize * 0.5f, (float)Constants.RegionSize * 0.5f, 0);
2539 }
4048 } 2540 }
4049 } 2541 }
4050 2542*/
4051 public override void Dispose() 2543 public override void Dispose()
4052 { 2544 {
4053 _worldInitialized = false; 2545 if (m_meshWorker != null)
4054 2546 m_meshWorker.Stop();
4055 m_rayCastManager.Dispose();
4056 m_rayCastManager = null;
4057 2547
4058 lock (OdeLock) 2548 lock (OdeLock)
4059 { 2549 {
2550 m_rayCastManager.Dispose();
2551 m_rayCastManager = null;
2552
4060 lock (_prims) 2553 lock (_prims)
4061 { 2554 {
2555 ChangesQueue.Clear();
4062 foreach (OdePrim prm in _prims) 2556 foreach (OdePrim prm in _prims)
4063 { 2557 {
4064 RemovePrim(prm); 2558 prm.DoAChange(changes.Remove, null);
2559 _collisionEventPrim.Remove(prm);
4065 } 2560 }
2561 _prims.Clear();
2562 }
2563
2564 OdeCharacter[] chtorem;
2565 lock (_characters)
2566 {
2567 chtorem = new OdeCharacter[_characters.Count];
2568 _characters.CopyTo(chtorem);
4066 } 2569 }
4067 2570
4068 //foreach (OdeCharacter act in _characters) 2571 ChangesQueue.Clear();
4069 //{ 2572 foreach (OdeCharacter ch in chtorem)
4070 //RemoveAvatar(act); 2573 ch.DoAChange(changes.Remove, null);
4071 //} 2574
2575
2576 foreach (IntPtr GroundGeom in RegionTerrain.Values)
2577 {
2578 if (GroundGeom != IntPtr.Zero)
2579 d.GeomDestroy(GroundGeom);
2580 }
2581
2582
2583 RegionTerrain.Clear();
2584
2585 if (TerrainHeightFieldHeightsHandlers.Count > 0)
2586 {
2587 foreach (GCHandle gch in TerrainHeightFieldHeightsHandlers.Values)
2588 {
2589 if (gch.IsAllocated)
2590 gch.Free();
2591 }
2592 }
2593
2594 TerrainHeightFieldHeightsHandlers.Clear();
2595 TerrainHeightFieldHeights.Clear();
2596/*
2597 if (WaterGeom != IntPtr.Zero)
2598 {
2599 d.GeomDestroy(WaterGeom);
2600 WaterGeom = IntPtr.Zero;
2601 if (WaterHeightmapData != IntPtr.Zero)
2602 d.GeomHeightfieldDataDestroy(WaterHeightmapData);
2603 WaterHeightmapData = IntPtr.Zero;
2604
2605 if (WaterMapHandler.IsAllocated)
2606 WaterMapHandler.Free();
2607 }
2608*/
2609 if (ContactgeomsArray != IntPtr.Zero)
2610 Marshal.FreeHGlobal(ContactgeomsArray);
2611 if (GlobalContactsArray != IntPtr.Zero)
2612 Marshal.FreeHGlobal(GlobalContactsArray);
2613
2614
4072 d.WorldDestroy(world); 2615 d.WorldDestroy(world);
2616 world = IntPtr.Zero;
4073 //d.CloseODE(); 2617 //d.CloseODE();
4074 } 2618 }
4075
4076 } 2619 }
4077 2620
4078 public override Dictionary<uint, float> GetTopColliders() 2621 public override Dictionary<uint, float> GetTopColliders()
4079 { 2622 {
4080 Dictionary<uint, float> topColliders; 2623 Dictionary<uint, float> returncolliders = new Dictionary<uint, float>();
4081 2624 int cnt = 0;
4082 lock (_prims) 2625 lock (_prims)
4083 { 2626 {
4084 List<OdePrim> orderedPrims = new List<OdePrim>(_prims); 2627 foreach (OdePrim prm in _prims)
4085 orderedPrims.OrderByDescending(p => p.CollisionScore).Take(25); 2628 {
4086 topColliders = orderedPrims.ToDictionary(p => p.LocalID, p => p.CollisionScore); 2629 if (prm.CollisionScore > 0)
4087 2630 {
4088 foreach (OdePrim p in _prims) 2631 returncolliders.Add(prm.LocalID, prm.CollisionScore);
4089 p.CollisionScore = 0; 2632 cnt++;
2633 prm.CollisionScore = 0f;
2634 if (cnt > 25)
2635 {
2636 break;
2637 }
2638 }
2639 }
4090 } 2640 }
4091 2641 return returncolliders;
4092 return topColliders;
4093 } 2642 }
4094 2643
4095 public override bool SupportsRayCast() 2644 public override bool SupportsRayCast()
@@ -4113,6 +2662,7 @@ namespace OpenSim.Region.Physics.OdePlugin
4113 } 2662 }
4114 } 2663 }
4115 2664
2665 // don't like this
4116 public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int Count) 2666 public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int Count)
4117 { 2667 {
4118 ContactResult[] ourResults = null; 2668 ContactResult[] ourResults = null;
@@ -4129,182 +2679,107 @@ namespace OpenSim.Region.Physics.OdePlugin
4129 waitTime++; 2679 waitTime++;
4130 } 2680 }
4131 if (ourResults == null) 2681 if (ourResults == null)
4132 return new List<ContactResult> (); 2682 return new List<ContactResult>();
4133 return new List<ContactResult>(ourResults); 2683 return new List<ContactResult>(ourResults);
4134 } 2684 }
4135 2685
4136#if USE_DRAWSTUFF 2686 public override bool SuportsRaycastWorldFiltered()
4137 // Keyboard callback
4138 public void command(int cmd)
4139 { 2687 {
4140 IntPtr geom; 2688 return true;
4141 d.Mass mass;
4142 d.Vector3 sides = new d.Vector3(d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f);
4143
4144
4145
4146 Char ch = Char.ToLower((Char)cmd);
4147 switch ((Char)ch)
4148 {
4149 case 'w':
4150 try
4151 {
4152 Vector3 rotate = (new Vector3(1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD));
4153
4154 xyz.X += rotate.X; xyz.Y += rotate.Y; xyz.Z += rotate.Z;
4155 ds.SetViewpoint(ref xyz, ref hpr);
4156 }
4157 catch (ArgumentException)
4158 { hpr.X = 0; }
4159 break;
4160
4161 case 'a':
4162 hpr.X++;
4163 ds.SetViewpoint(ref xyz, ref hpr);
4164 break;
4165
4166 case 's':
4167 try
4168 {
4169 Vector3 rotate2 = (new Vector3(-1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD));
4170
4171 xyz.X += rotate2.X; xyz.Y += rotate2.Y; xyz.Z += rotate2.Z;
4172 ds.SetViewpoint(ref xyz, ref hpr);
4173 }
4174 catch (ArgumentException)
4175 { hpr.X = 0; }
4176 break;
4177 case 'd':
4178 hpr.X--;
4179 ds.SetViewpoint(ref xyz, ref hpr);
4180 break;
4181 case 'r':
4182 xyz.Z++;
4183 ds.SetViewpoint(ref xyz, ref hpr);
4184 break;
4185 case 'f':
4186 xyz.Z--;
4187 ds.SetViewpoint(ref xyz, ref hpr);
4188 break;
4189 case 'e':
4190 xyz.Y++;
4191 ds.SetViewpoint(ref xyz, ref hpr);
4192 break;
4193 case 'q':
4194 xyz.Y--;
4195 ds.SetViewpoint(ref xyz, ref hpr);
4196 break;
4197 }
4198 } 2689 }
4199 2690
4200 public void step(int pause) 2691 public override object RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags filter)
4201 { 2692 {
4202 2693 object SyncObject = new object();
4203 ds.SetColor(1.0f, 1.0f, 0.0f); 2694 List<ContactResult> ourresults = new List<ContactResult>();
4204 ds.SetTexture(ds.Texture.Wood); 2695
4205 lock (_prims) 2696 RayCallback retMethod = delegate(List<ContactResult> results)
4206 { 2697 {
4207 foreach (OdePrim prm in _prims) 2698 lock (SyncObject)
4208 { 2699 {
4209 //IntPtr body = d.GeomGetBody(prm.prim_geom); 2700 ourresults = results;
4210 if (prm.prim_geom != IntPtr.Zero) 2701 Monitor.PulseAll(SyncObject);
4211 {
4212 d.Vector3 pos;
4213 d.GeomCopyPosition(prm.prim_geom, out pos);
4214 //d.BodyCopyPosition(body, out pos);
4215
4216 d.Matrix3 R;
4217 d.GeomCopyRotation(prm.prim_geom, out R);
4218 //d.BodyCopyRotation(body, out R);
4219
4220
4221 d.Vector3 sides = new d.Vector3();
4222 sides.X = prm.Size.X;
4223 sides.Y = prm.Size.Y;
4224 sides.Z = prm.Size.Z;
4225
4226 ds.DrawBox(ref pos, ref R, ref sides);
4227 }
4228 } 2702 }
4229 } 2703 };
4230 ds.SetColor(1.0f, 0.0f, 0.0f);
4231 2704
4232 foreach (OdeCharacter chr in _characters) 2705 lock (SyncObject)
4233 { 2706 {
4234 if (chr.Shell != IntPtr.Zero) 2707 m_rayCastManager.QueueRequest(position, direction, length, Count,filter, retMethod);
4235 { 2708 if (!Monitor.Wait(SyncObject, 500))
4236 IntPtr body = d.GeomGetBody(chr.Shell); 2709 return null;
4237 2710 else
4238 d.Vector3 pos; 2711 return ourresults;
4239 d.GeomCopyPosition(chr.Shell, out pos);
4240 //d.BodyCopyPosition(body, out pos);
4241
4242 d.Matrix3 R;
4243 d.GeomCopyRotation(chr.Shell, out R);
4244 //d.BodyCopyRotation(body, out R);
4245
4246 ds.DrawCapsule(ref pos, ref R, chr.Size.Z, 0.35f);
4247 d.Vector3 sides = new d.Vector3();
4248 sides.X = 0.5f;
4249 sides.Y = 0.5f;
4250 sides.Z = 0.5f;
4251
4252 ds.DrawBox(ref pos, ref R, ref sides);
4253 }
4254 } 2712 }
4255 } 2713 }
4256 2714
4257 public void start(int unused) 2715 public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
4258 { 2716 {
4259 ds.SetViewpoint(ref xyz, ref hpr); 2717 if (retMethod != null && actor !=null)
2718 {
2719 IntPtr geom;
2720 if (actor is OdePrim)
2721 geom = ((OdePrim)actor).prim_geom;
2722 else if (actor is OdeCharacter)
2723 geom = ((OdePrim)actor).prim_geom;
2724 else
2725 return;
2726 if (geom == IntPtr.Zero)
2727 return;
2728 m_rayCastManager.QueueRequest(geom, position, direction, length, retMethod);
2729 }
4260 } 2730 }
4261#endif
4262 2731
4263 public override Dictionary<string, float> GetStats() 2732 public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod)
4264 { 2733 {
4265 if (!CollectStats) 2734 if (retMethod != null && actor != null)
4266 return null;
4267
4268 Dictionary<string, float> returnStats;
4269
4270 lock (OdeLock)
4271 { 2735 {
4272 returnStats = new Dictionary<string, float>(m_stats); 2736 IntPtr geom;
4273 2737 if (actor is OdePrim)
4274 // FIXME: This is a SUPER DUMB HACK until we can establish stats that aren't subject to a division by 2738 geom = ((OdePrim)actor).prim_geom;
4275 // 3 from the SimStatsReporter. 2739 else if (actor is OdeCharacter)
4276 returnStats[ODETotalAvatarsStatName] = _characters.Count * 3; 2740 geom = ((OdePrim)actor).prim_geom;
4277 returnStats[ODETotalPrimsStatName] = _prims.Count * 3; 2741 else
4278 returnStats[ODEActivePrimsStatName] = _activeprims.Count * 3; 2742 return;
2743 if (geom == IntPtr.Zero)
2744 return;
4279 2745
4280 InitializeExtraStats(); 2746 m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod);
4281 } 2747 }
4282
4283 returnStats[ODEOtherCollisionFrameMsStatName]
4284 = returnStats[ODEOtherCollisionFrameMsStatName]
4285 - returnStats[ODENativeSpaceCollisionFrameMsStatName]
4286 - returnStats[ODENativeGeomCollisionFrameMsStatName];
4287
4288 return returnStats;
4289 } 2748 }
4290 2749
4291 private void InitializeExtraStats() 2750 // don't like this
2751 public override List<ContactResult> RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count)
4292 { 2752 {
4293 m_stats[ODETotalFrameMsStatName] = 0; 2753 if (actor != null)
4294 m_stats[ODEAvatarTaintMsStatName] = 0; 2754 {
4295 m_stats[ODEPrimTaintMsStatName] = 0; 2755 IntPtr geom;
4296 m_stats[ODEAvatarForcesFrameMsStatName] = 0; 2756 if (actor is OdePrim)
4297 m_stats[ODEPrimForcesFrameMsStatName] = 0; 2757 geom = ((OdePrim)actor).prim_geom;
4298 m_stats[ODERaycastingFrameMsStatName] = 0; 2758 else if (actor is OdeCharacter)
4299 m_stats[ODENativeStepFrameMsStatName] = 0; 2759 geom = ((OdePrim)actor).prim_geom;
4300 m_stats[ODENativeSpaceCollisionFrameMsStatName] = 0; 2760 else
4301 m_stats[ODENativeGeomCollisionFrameMsStatName] = 0; 2761 return new List<ContactResult>();
4302 m_stats[ODEOtherCollisionFrameMsStatName] = 0; 2762 if (geom == IntPtr.Zero)
4303 m_stats[ODECollisionNotificationFrameMsStatName] = 0; 2763 return new List<ContactResult>();
4304 m_stats[ODEAvatarContactsStatsName] = 0; 2764
4305 m_stats[ODEPrimContactsStatName] = 0; 2765 ContactResult[] ourResults = null;
4306 m_stats[ODEAvatarUpdateFrameMsStatName] = 0; 2766 RayCallback retMethod = delegate(List<ContactResult> results)
4307 m_stats[ODEPrimUpdateFrameMsStatName] = 0; 2767 {
2768 ourResults = new ContactResult[results.Count];
2769 results.CopyTo(ourResults, 0);
2770 };
2771 int waitTime = 0;
2772 m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod);
2773 while (ourResults == null && waitTime < 1000)
2774 {
2775 Thread.Sleep(1);
2776 waitTime++;
2777 }
2778 if (ourResults == null)
2779 return new List<ContactResult>();
2780 return new List<ContactResult>(ourResults);
2781 }
2782 return new List<ContactResult>();
4308 } 2783 }
4309 } 2784 }
4310} \ No newline at end of file 2785}
generated by cgit v1.1 at 2024-10-02 16:28:53 +0000