aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim')
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs7718
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs126
2 files changed, 3959 insertions, 3885 deletions
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
index bacd604..c4dc793 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs
@@ -1,1807 +1,1855 @@
1/* 1/*
2 * Copyright (c) Contributors, http://opensimulator.org/ 2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders. 3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 * 4 *
5 * Redistribution and use in source and binary forms, with or without 5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met: 6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright 7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer. 8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright 9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the 10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution. 11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the 12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products 13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission. 14 * derived from this software without specific prior written permission.
15 * 15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY 16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY 19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */ 26 */
27 27
28/* Revision 2011/12 by Ubit Umarov 28/* Revision 2011/12 by Ubit Umarov
29 * 29 *
30 * 30 *
31 */ 31 */
32 32
33/* 33/*
34 * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces 34 * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces
35 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: 35 * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
36 * ODEPrim.cs contains methods dealing with Prim editing, Prim 36 * ODEPrim.cs contains methods dealing with Prim editing, Prim
37 * characteristics and Kinetic motion. 37 * characteristics and Kinetic motion.
38 * ODEDynamics.cs contains methods dealing with Prim Physical motion 38 * ODEDynamics.cs contains methods dealing with Prim Physical motion
39 * (dynamics) and the associated settings. Old Linear and angular 39 * (dynamics) and the associated settings. Old Linear and angular
40 * motors for dynamic motion have been replace with MoveLinear() 40 * motors for dynamic motion have been replace with MoveLinear()
41 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic 41 * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
42 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to 42 * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
43 * switch between 'VEHICLE' parameter use and general dynamics 43 * switch between 'VEHICLE' parameter use and general dynamics
44 * settings use. 44 * settings use.
45 */ 45 */
46 46
47//#define SPAM 47//#define SPAM
48 48
49using System; 49using System;
50using System.Collections.Generic; 50using System.Collections.Generic;
51using System.Reflection; 51using System.Reflection;
52using System.Runtime.InteropServices; 52using System.Runtime.InteropServices;
53using System.Threading; 53using System.Threading;
54using log4net; 54using log4net;
55using OpenMetaverse; 55using OpenMetaverse;
56using OdeAPI; 56using OdeAPI;
57using OpenSim.Framework; 57using OpenSim.Framework;
58using OpenSim.Region.Physics.Manager; 58using OpenSim.Region.Physics.Manager;
59 59
60 60
61namespace OpenSim.Region.Physics.OdePlugin 61namespace OpenSim.Region.Physics.OdePlugin
62{ 62{
63 public class OdePrim : PhysicsActor 63 public class OdePrim : PhysicsActor
64 { 64 {
65 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); 65 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
66 66
67 private bool m_isphysical; 67 private bool m_isphysical;
68 private bool m_fakeisphysical; 68 private bool m_fakeisphysical;
69 private bool m_isphantom; 69 private bool m_isphantom;
70 private bool m_fakeisphantom; 70 private bool m_fakeisphantom;
71 71
72 protected bool m_building; 72 protected bool m_building;
73 protected bool m_forcePosOrRotation; 73 protected bool m_forcePosOrRotation;
74 74
75 private Quaternion m_lastorientation = new Quaternion(); 75 private Quaternion m_lastorientation = new Quaternion();
76 private Quaternion _orientation; 76 private Quaternion _orientation;
77 77
78 private Vector3 _position; 78 private Vector3 _position;
79 private Vector3 _velocity; 79 private Vector3 _velocity;
80 private Vector3 _torque; 80 private Vector3 _torque;
81 private Vector3 m_lastVelocity; 81 private Vector3 m_lastVelocity;
82 private Vector3 m_lastposition; 82 private Vector3 m_lastposition;
83 private Vector3 m_rotationalVelocity; 83 private Vector3 m_rotationalVelocity;
84 private Vector3 _size; 84 private Vector3 _size;
85 private Vector3 _acceleration; 85 private Vector3 _acceleration;
86 private Vector3 m_angularlock = Vector3.One; 86 private Vector3 m_angularlock = Vector3.One;
87 private IntPtr Amotor = IntPtr.Zero; 87 private IntPtr Amotor = IntPtr.Zero;
88 88
89 private Vector3 m_force; 89 private Vector3 m_force;
90 private Vector3 m_forceacc; 90 private Vector3 m_forceacc;
91 private Vector3 m_angularForceacc; 91 private Vector3 m_angularForceacc;
92 92
93 private Vector3 m_PIDTarget; 93 private Vector3 m_PIDTarget;
94 private float m_PIDTau; 94 private float m_PIDTau;
95 private float PID_D = 35f; 95 private float PID_D = 35f;
96 private float PID_G = 25f; 96 private float PID_G = 25f;
97 private bool m_usePID; 97 private bool m_usePID;
98 98
99 // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau), 99 // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
100 // and are for non-VEHICLES only. 100 // and are for non-VEHICLES only.
101 101
102 private float m_PIDHoverHeight; 102 private float m_PIDHoverHeight;
103 private float m_PIDHoverTau; 103 private float m_PIDHoverTau;
104 private bool m_useHoverPID; 104 private bool m_useHoverPID;
105 private PIDHoverType m_PIDHoverType = PIDHoverType.Ground; 105 private PIDHoverType m_PIDHoverType = PIDHoverType.Ground;
106 private float m_targetHoverHeight; 106 private float m_targetHoverHeight;
107 private float m_groundHeight; 107 private float m_groundHeight;
108 private float m_waterHeight; 108 private float m_waterHeight;
109 private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle. 109 private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
110 110
111 private int body_autodisable_frames = 20; 111 private int body_autodisable_frames = 20;
112 112
113 private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom 113 private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom
114 | CollisionCategories.Space 114 | CollisionCategories.Space
115 | CollisionCategories.Body 115 | CollisionCategories.Body
116 | CollisionCategories.Character 116 | CollisionCategories.Character
117 ); 117 );
118// private bool m_collidesLand = true; 118// private bool m_collidesLand = true;
119 private bool m_collidesWater; 119 private bool m_collidesWater;
120 public bool m_returnCollisions; 120 public bool m_returnCollisions;
121 private bool m_softcolide; 121 private bool m_softcolide;
122 122
123 private bool m_NoColide; // for now only for internal use for bad meshs 123 private bool m_NoColide; // for now only for internal use for bad meshs
124 124
125 // Default we're a Geometry 125 // Default we're a Geometry
126 private CollisionCategories m_collisionCategories = (CollisionCategories.Geom); 126 private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
127 127
128 // Default, Collide with Other Geometries, spaces and Bodies 128 // Default, Collide with Other Geometries, spaces and Bodies
129 private CollisionCategories m_collisionFlags = m_default_collisionFlags; 129 private CollisionCategories m_collisionFlags = m_default_collisionFlags;
130 130
131 public bool m_disabled; 131 public bool m_disabled;
132 132
133 133 public uint m_localID;
134 public uint m_localID; 134
135 135 private IMesh m_mesh;
136 private PrimitiveBaseShape _pbs; 136 private object m_meshlock = new object();
137 public OdeScene _parent_scene; 137 private PrimitiveBaseShape _pbs;
138 138 public OdeScene _parent_scene;
139 /// <summary> 139
140 /// The physics space which contains prim geometry 140 /// <summary>
141 /// </summary> 141 /// The physics space which contains prim geometry
142 public IntPtr m_targetSpace = IntPtr.Zero; 142 /// </summary>
143 143 public IntPtr m_targetSpace = IntPtr.Zero;
144 public IntPtr prim_geom; 144
145 public IntPtr _triMeshData; 145 public IntPtr prim_geom;
146 146 public IntPtr _triMeshData;
147 private PhysicsActor _parent; 147
148 148 private PhysicsActor _parent;
149 private List<OdePrim> childrenPrim = new List<OdePrim>(); 149
150 150 private List<OdePrim> childrenPrim = new List<OdePrim>();
151 private bool m_iscolliding; 151
152 152 private bool m_iscolliding;
153 public bool m_isSelected; 153
154 private bool m_delaySelect; 154 public bool m_isSelected;
155 private bool m_lastdoneSelected; 155 private bool m_delaySelect;
156 public bool m_outbounds; 156 private bool m_lastdoneSelected;
157 157 public bool m_outbounds;
158 internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively 158
159 159 internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
160 private bool m_throttleUpdates; 160
161 private int throttleCounter; 161 private bool m_throttleUpdates;
162 public float m_collisionscore; 162 private int throttleCounter;
163 int m_colliderfilter = 0; 163 public float m_collisionscore;
164 164 int m_colliderfilter = 0;
165 public IntPtr collide_geom; // for objects: geom if single prim space it linkset 165
166 166 public IntPtr collide_geom; // for objects: geom if single prim space it linkset
167 private float m_density = 10.000006836f; // Aluminum g/cm3; 167
168 private byte m_shapetype; 168 private float m_density = 10.000006836f; // Aluminum g/cm3;
169 public bool _zeroFlag; 169 private byte m_shapetype;
170 private bool m_lastUpdateSent; 170 public bool _zeroFlag;
171 171 private bool m_lastUpdateSent;
172 public IntPtr Body = IntPtr.Zero; 172
173 public String Name { get; private set; } 173 public IntPtr Body = IntPtr.Zero;
174 private Vector3 _target_velocity; 174 public String Name { get; private set; }
175 175 private Vector3 _target_velocity;
176 public Vector3 primOOBsize; // prim real dimensions from mesh 176
177 public Vector3 primOOBoffset; // its centroid out of mesh or rest aabb 177 public Vector3 primOOBsize; // prim real dimensions from mesh
178 public float primOOBradiusSQ; 178 public Vector3 primOOBoffset; // its centroid out of mesh or rest aabb
179 public d.Mass primdMass; // prim inertia information on it's own referencial 179 public float primOOBradiusSQ;
180 float primMass; // prim own mass 180 public d.Mass primdMass; // prim inertia information on it's own referencial
181 float _mass; // object mass acording to case 181 float primMass; // prim own mass
182 private bool hasOOBoffsetFromMesh = false; // if true we did compute it form mesh centroid, else from aabb 182 float _mass; // object mass acording to case
183 183 private bool hasOOBoffsetFromMesh = false; // if true we did compute it form mesh centroid, else from aabb
184 public int givefakepos = 0; 184
185 private Vector3 fakepos; 185 public int givefakepos = 0;
186 public int givefakeori = 0; 186 private Vector3 fakepos;
187 private Quaternion fakeori; 187 public int givefakeori = 0;
188 188 private Quaternion fakeori;
189 public int m_eventsubscription; 189
190 private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate(); 190 public int m_eventsubscription;
191 191 private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate();
192 public volatile bool childPrim; 192
193 193 public volatile bool childPrim;
194 public ODEDynamics m_vehicle; 194
195 195 public ODEDynamics m_vehicle;
196 internal int m_material = (int)Material.Wood; 196
197 private float mu; 197 internal int m_material = (int)Material.Wood;
198 private float bounce; 198 private float mu;
199 199 private float bounce;
200 /// <summary> 200
201 /// Is this prim subject to physics? Even if not, it's still solid for collision purposes. 201 /// <summary>
202 /// </summary> 202 /// Is this prim subject to physics? Even if not, it's still solid for collision purposes.
203 public override bool IsPhysical // this is not reliable for internal use 203 /// </summary>
204 { 204 public override bool IsPhysical // this is not reliable for internal use
205 get { return m_fakeisphysical; } 205 {
206 set 206 get { return m_fakeisphysical; }
207 { 207 set
208 m_fakeisphysical = value; // we show imediatly to outside that we changed physical 208 {
209 // and also to stop imediatly some updates 209 m_fakeisphysical = value; // we show imediatly to outside that we changed physical
210 // but real change will only happen in taintprocessing 210 // and also to stop imediatly some updates
211 211 // but real change will only happen in taintprocessing
212 if (!value) // Zero the remembered last velocity 212
213 m_lastVelocity = Vector3.Zero; 213 if (!value) // Zero the remembered last velocity
214 AddChange(changes.Physical, value); 214 m_lastVelocity = Vector3.Zero;
215 } 215 AddChange(changes.Physical, value);
216 } 216 }
217 217 }
218 public override bool Phantom // this is not reliable for internal use 218
219 { 219 public override bool Phantom // this is not reliable for internal use
220 get { return m_fakeisphantom; } 220 {
221 set 221 get { return m_fakeisphantom; }
222 { 222 set
223 m_fakeisphantom = value; // we show imediatly to outside that we changed physical 223 {
224 // and also to stop imediatly some updates 224 m_fakeisphantom = value; // we show imediatly to outside that we changed physical
225 // but real change will only happen in taintprocessing 225 // and also to stop imediatly some updates
226 226 // but real change will only happen in taintprocessing
227 AddChange(changes.Phantom, value); 227
228 } 228 AddChange(changes.Phantom, value);
229 } 229 }
230 230 }
231 public override bool Building // this is not reliable for internal use 231
232 { 232 public override bool Building // this is not reliable for internal use
233 get { return m_building; } 233 {
234 set 234 get { return m_building; }
235 { 235 set
236 if (value) 236 {
237 m_building = true; 237 if (value)
238 AddChange(changes.building, value); 238 m_building = true;
239 } 239 AddChange(changes.building, value);
240 } 240 }
241 241 }
242 public override void getContactData(ref ContactData cdata) 242
243 { 243 public override void getContactData(ref ContactData cdata)
244 cdata.mu = mu; 244 {
245 cdata.bounce = bounce; 245 cdata.mu = mu;
246 246 cdata.bounce = bounce;
247 // cdata.softcolide = m_softcolide; 247
248 cdata.softcolide = false; 248 // cdata.softcolide = m_softcolide;
249 249 cdata.softcolide = false;
250 if (m_isphysical) 250
251 { 251 if (m_isphysical)
252 ODEDynamics veh; 252 {
253 if (_parent != null) 253 ODEDynamics veh;
254 veh = ((OdePrim)_parent).m_vehicle; 254 if (_parent != null)
255 else 255 veh = ((OdePrim)_parent).m_vehicle;
256 veh = m_vehicle; 256 else
257 257 veh = m_vehicle;
258 if (veh != null && veh.Type != Vehicle.TYPE_NONE) 258
259 cdata.mu *= veh.FrictionFactor; 259 if (veh != null && veh.Type != Vehicle.TYPE_NONE)
260 } 260 cdata.mu *= veh.FrictionFactor;
261 } 261 }
262 262 }
263 public override int PhysicsActorType 263
264 { 264 public override int PhysicsActorType
265 get { return (int)ActorTypes.Prim; } 265 {
266 set { return; } 266 get { return (int)ActorTypes.Prim; }
267 } 267 set { return; }
268 268 }
269 public override bool SetAlwaysRun 269
270 { 270 public override bool SetAlwaysRun
271 get { return false; } 271 {
272 set { return; } 272 get { return false; }
273 } 273 set { return; }
274 274 }
275 public override uint LocalID 275
276 { 276 public override uint LocalID
277 get 277 {
278 { 278 get
279 return m_localID; 279 {
280 } 280 return m_localID;
281 set 281 }
282 { 282 set
283 //m_log.Info("[PHYSICS]: Setting TrackerID: " + value); 283 {
284 m_localID = value; 284 //m_log.Info("[PHYSICS]: Setting TrackerID: " + value);
285 } 285 m_localID = value;
286 } 286 }
287 287 }
288 public override bool Grabbed 288
289 { 289 public override bool Grabbed
290 set { return; } 290 {
291 } 291 set { return; }
292 292 }
293 public override bool Selected 293
294 { 294 public override bool Selected
295 set 295 {
296 { 296 set
297 if (value) 297 {
298 m_isSelected = value; // if true set imediatly to stop moves etc 298 if (value)
299 AddChange(changes.Selected, value); 299 m_isSelected = value; // if true set imediatly to stop moves etc
300 } 300 AddChange(changes.Selected, value);
301 } 301 }
302 302 }
303 public override bool Flying 303
304 { 304 public override bool Flying
305 // no flying prims for you 305 {
306 get { return false; } 306 // no flying prims for you
307 set { } 307 get { return false; }
308 } 308 set { }
309 309 }
310 public override bool IsColliding 310
311 { 311 public override bool IsColliding
312 get { return m_iscolliding; } 312 {
313 set 313 get { return m_iscolliding; }
314 { 314 set
315 if (value) 315 {
316 { 316 if (value)
317 m_colliderfilter += 2; 317 {
318 if (m_colliderfilter > 2) 318 m_colliderfilter += 2;
319 m_colliderfilter = 2; 319 if (m_colliderfilter > 2)
320 } 320 m_colliderfilter = 2;
321 else 321 }
322 { 322 else
323 m_colliderfilter--; 323 {
324 if (m_colliderfilter < 0) 324 m_colliderfilter--;
325 m_colliderfilter = 0; 325 if (m_colliderfilter < 0)
326 } 326 m_colliderfilter = 0;
327 327 }
328 if (m_colliderfilter == 0) 328
329 { 329 if (m_colliderfilter == 0)
330 m_softcolide = false; 330 {
331 m_iscolliding = false; 331 m_softcolide = false;
332 } 332 m_iscolliding = false;
333 else 333 }
334 m_iscolliding = true; 334 else
335 } 335 m_iscolliding = true;
336 } 336 }
337 337 }
338 public override bool CollidingGround 338
339 { 339 public override bool CollidingGround
340 get { return false; } 340 {
341 set { return; } 341 get { return false; }
342 } 342 set { return; }
343 343 }
344 public override bool CollidingObj 344
345 { 345 public override bool CollidingObj
346 get { return false; } 346 {
347 set { return; } 347 get { return false; }
348 } 348 set { return; }
349 349 }
350 public override bool ThrottleUpdates 350
351 { 351 public override bool ThrottleUpdates
352 get { return m_throttleUpdates; } 352 {
353 set { m_throttleUpdates = value; } 353 get { return m_throttleUpdates; }
354 } 354 set { m_throttleUpdates = value; }
355 355 }
356 public override bool Stopped 356
357 { 357 public override bool Stopped
358 get { return _zeroFlag; } 358 {
359 } 359 get { return _zeroFlag; }
360 360 }
361 public override Vector3 Position 361
362 { 362 public override Vector3 Position
363 get 363 {
364 { 364 get
365 if (givefakepos > 0) 365 {
366 return fakepos; 366 if (givefakepos > 0)
367 else 367 return fakepos;
368 return _position; 368 else
369 } 369 return _position;
370 370 }
371 set 371
372 { 372 set
373 fakepos = value; 373 {
374 givefakepos++; 374 fakepos = value;
375 AddChange(changes.Position, value); 375 givefakepos++;
376 } 376 AddChange(changes.Position, value);
377 } 377 }
378 378 }
379 public override Vector3 Size 379
380 { 380 public override Vector3 Size
381 get { return _size; } 381 {
382 set 382 get { return _size; }
383 { 383 set
384 if (value.IsFinite()) 384 {
385 { 385 if (value.IsFinite())
386 AddChange(changes.Size, value); 386 {
387 } 387 AddChange(changes.Size, value);
388 else 388 }
389 { 389 else
390 m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name); 390 {
391 } 391 m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name);
392 } 392 }
393 } 393 }
394 394 }
395 public override float Mass 395
396 { 396 public override float Mass
397 get { return _mass; } 397 {
398 } 398 get { return _mass; }
399 399 }
400 public override Vector3 Force 400
401 { 401 public override Vector3 Force
402 //get { return Vector3.Zero; } 402 {
403 get { return m_force; } 403 //get { return Vector3.Zero; }
404 set 404 get { return m_force; }
405 { 405 set
406 if (value.IsFinite()) 406 {
407 { 407 if (value.IsFinite())
408 AddChange(changes.Force, value); 408 {
409 } 409 AddChange(changes.Force, value);
410 else 410 }
411 { 411 else
412 m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name); 412 {
413 } 413 m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name);
414 } 414 }
415 } 415 }
416 416 }
417 public override void SetVolumeDetect(int param) 417
418 { 418 public override void SetVolumeDetect(int param)
419 AddChange(changes.VolumeDtc, (param != 0)); 419 {
420 } 420 AddChange(changes.VolumeDtc, (param != 0));
421 421 }
422 public override Vector3 GeometricCenter 422
423 { 423 public override Vector3 GeometricCenter
424 get 424 {
425 { 425 get
426 return Vector3.Zero; 426 {
427 } 427 return Vector3.Zero;
428 } 428 }
429 429 }
430 public override Vector3 CenterOfMass 430
431 { 431 public override Vector3 CenterOfMass
432 get 432 {
433 { 433 get
434 d.Vector3 dtmp; 434 {
435 if (IsPhysical && !childPrim && Body != IntPtr.Zero) 435 d.Vector3 dtmp;
436 { 436 if (IsPhysical && !childPrim && Body != IntPtr.Zero)
437 dtmp = d.BodyGetPosition(Body); 437 {
438 return new Vector3(dtmp.X, dtmp.Y, dtmp.Z); 438 dtmp = d.BodyGetPosition(Body);
439 } 439 return new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
440 else if (prim_geom != IntPtr.Zero) 440 }
441 { 441 else if (prim_geom != IntPtr.Zero)
442 d.Quaternion dq; 442 {
443 d.GeomCopyQuaternion(prim_geom, out dq); 443 d.Quaternion dq;
444 Quaternion q; 444 d.GeomCopyQuaternion(prim_geom, out dq);
445 q.X = dq.X; 445 Quaternion q;
446 q.Y = dq.Y; 446 q.X = dq.X;
447 q.Z = dq.Z; 447 q.Y = dq.Y;
448 q.W = dq.W; 448 q.Z = dq.Z;
449 449 q.W = dq.W;
450 Vector3 vtmp = primOOBoffset * q; 450
451 dtmp = d.GeomGetPosition(prim_geom); 451 Vector3 vtmp = primOOBoffset * q;
452 return new Vector3(dtmp.X + vtmp.X, dtmp.Y + vtmp.Y, dtmp.Z + vtmp.Z); 452 dtmp = d.GeomGetPosition(prim_geom);
453 } 453 return new Vector3(dtmp.X + vtmp.X, dtmp.Y + vtmp.Y, dtmp.Z + vtmp.Z);
454 else 454 }
455 return Vector3.Zero; 455 else
456 } 456 return Vector3.Zero;
457 } 457 }
458 /* 458 }
459 public override Vector3 PrimOOBsize 459 /*
460 { 460 public override Vector3 PrimOOBsize
461 get 461 {
462 { 462 get
463 return primOOBsize; 463 {
464 } 464 return primOOBsize;
465 } 465 }
466 466 }
467 public override Vector3 PrimOOBoffset 467
468 { 468 public override Vector3 PrimOOBoffset
469 get 469 {
470 { 470 get
471 return primOOBoffset; 471 {
472 } 472 return primOOBoffset;
473 } 473 }
474 474 }
475 public override float PrimOOBRadiusSQ 475
476 { 476 public override float PrimOOBRadiusSQ
477 get 477 {
478 { 478 get
479 return primOOBradiusSQ; 479 {
480 } 480 return primOOBradiusSQ;
481 } 481 }
482 */ 482 }
483 public override PrimitiveBaseShape Shape 483 */
484 { 484 public override PrimitiveBaseShape Shape
485 set 485 {
486 { 486 set
487 AddChange(changes.Shape, value); 487 {
488 } 488/*
489 } 489 IMesh mesh = null;
490 490 if (_parent_scene.needsMeshing(value))
491 public override byte PhysicsShapeType 491 {
492 { 492 bool convex;
493 get 493 if (m_shapetype == 0)
494 { 494 convex = false;
495 return m_shapetype; 495 else
496 } 496 convex = true;
497 set 497 mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true, convex);
498 { 498 }
499 m_shapetype = value; 499
500 AddChange(changes.Shape, null); 500 if (mesh != null)
501 } 501 {
502 } 502 lock (m_meshlock)
503 503 m_mesh = mesh;
504 504 }
505 public override Vector3 Velocity 505*/
506 { 506 AddChange(changes.Shape, value);
507 get 507 }
508 { 508 }
509 if (_zeroFlag) 509
510 return Vector3.Zero; 510 public override byte PhysicsShapeType
511 return _velocity; 511 {
512 } 512 get
513 set 513 {
514 { 514 return m_shapetype;
515 if (value.IsFinite()) 515 }
516 { 516 set
517 AddChange(changes.Velocity, value); 517 {
518 // _velocity = value; 518 m_shapetype = value;
519 519 AddChange(changes.Shape, null);
520 } 520 }
521 else 521 }
522 { 522
523 m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name); 523
524 } 524 public override Vector3 Velocity
525 525 {
526 } 526 get
527 } 527 {
528 528 if (_zeroFlag)
529 public override Vector3 Torque 529 return Vector3.Zero;
530 { 530 return _velocity;
531 get 531 }
532 { 532 set
533 if (!IsPhysical || Body == IntPtr.Zero) 533 {
534 return Vector3.Zero; 534 if (value.IsFinite())
535 535 {
536 return _torque; 536 AddChange(changes.Velocity, value);
537 } 537 // _velocity = value;
538 538
539 set 539 }
540 { 540 else
541 if (value.IsFinite()) 541 {
542 { 542 m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name);
543 AddChange(changes.Torque, value); 543 }
544 } 544
545 else 545 }
546 { 546 }
547 m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name); 547
548 } 548 public override Vector3 Torque
549 } 549 {
550 } 550 get
551 551 {
552 public override float CollisionScore 552 if (!IsPhysical || Body == IntPtr.Zero)
553 { 553 return Vector3.Zero;
554 get { return m_collisionscore; } 554
555 set { m_collisionscore = value; } 555 return _torque;
556 } 556 }
557 557
558 public override bool Kinematic 558 set
559 { 559 {
560 get { return false; } 560 if (value.IsFinite())
561 set { } 561 {
562 } 562 AddChange(changes.Torque, value);
563 563 }
564 public override Quaternion Orientation 564 else
565 { 565 {
566 get 566 m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name);
567 { 567 }
568 if (givefakeori > 0) 568 }
569 return fakeori; 569 }
570 else 570
571 571 public override float CollisionScore
572 return _orientation; 572 {
573 } 573 get { return m_collisionscore; }
574 set 574 set { m_collisionscore = value; }
575 { 575 }
576 if (QuaternionIsFinite(value)) 576
577 { 577 public override bool Kinematic
578 fakeori = value; 578 {
579 givefakeori++; 579 get { return false; }
580 AddChange(changes.Orientation, value); 580 set { }
581 } 581 }
582 else 582
583 m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name); 583 public override Quaternion Orientation
584 584 {
585 } 585 get
586 } 586 {
587 587 if (givefakeori > 0)
588 public override Vector3 Acceleration 588 return fakeori;
589 { 589 else
590 get { return _acceleration; } 590
591 set { } 591 return _orientation;
592 } 592 }
593 593 set
594 public override Vector3 RotationalVelocity 594 {
595 { 595 if (QuaternionIsFinite(value))
596 get 596 {
597 { 597 fakeori = value;
598 Vector3 pv = Vector3.Zero; 598 givefakeori++;
599 if (_zeroFlag) 599 AddChange(changes.Orientation, value);
600 return pv; 600 }
601 m_lastUpdateSent = false; 601 else
602 602 m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name);
603 if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) 603
604 return pv; 604 }
605 605 }
606 return m_rotationalVelocity; 606
607 } 607 public override Vector3 Acceleration
608 set 608 {
609 { 609 get { return _acceleration; }
610 if (value.IsFinite()) 610 set { }
611 { 611 }
612 m_rotationalVelocity = value; 612
613 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) 613 public override Vector3 RotationalVelocity
614 d.BodyEnable(Body); 614 {
615 } 615 get
616 else 616 {
617 { 617 Vector3 pv = Vector3.Zero;
618 m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name); 618 if (_zeroFlag)
619 } 619 return pv;
620 } 620 m_lastUpdateSent = false;
621 } 621
622 622 if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f))
623 623 return pv;
624 public override float Buoyancy 624
625 { 625 return m_rotationalVelocity;
626 get { return m_buoyancy; } 626 }
627 set 627 set
628 { 628 {
629 m_buoyancy = value; 629 if (value.IsFinite())
630 } 630 {
631 } 631 m_rotationalVelocity = value;
632 632 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
633 public override bool FloatOnWater 633 d.BodyEnable(Body);
634 { 634 }
635 set 635 else
636 { 636 {
637 AddChange(changes.CollidesWater, value); 637 m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name);
638 } 638 }
639 } 639 }
640 640 }
641 public override Vector3 PIDTarget 641
642 { 642
643 set 643 public override float Buoyancy
644 { 644 {
645 if (value.IsFinite()) 645 get { return m_buoyancy; }
646 { 646 set
647 m_PIDTarget = value; 647 {
648 } 648 m_buoyancy = value;
649 else 649 }
650 m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name); 650 }
651 } 651
652 } 652 public override bool FloatOnWater
653 653 {
654 public override bool PIDActive { set { m_usePID = value; } } 654 set
655 public override float PIDTau { set { m_PIDTau = value; } } 655 {
656 656 AddChange(changes.CollidesWater, value);
657 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } } 657 }
658 public override bool PIDHoverActive { set { m_useHoverPID = value; } } 658 }
659 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } } 659
660 public override float PIDHoverTau { set { m_PIDHoverTau = value; } } 660 public override Vector3 PIDTarget
661 661 {
662 public override Quaternion APIDTarget { set { return; } } 662 set
663 663 {
664 public override bool APIDActive { set { return; } } 664 if (value.IsFinite())
665 665 {
666 public override float APIDStrength { set { return; } } 666 m_PIDTarget = value;
667 667 }
668 public override float APIDDamping { set { return; } } 668 else
669 669 m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name);
670 public override int VehicleType 670 }
671 { 671 }
672 // we may need to put a fake on this 672
673 get 673 public override bool PIDActive { set { m_usePID = value; } }
674 { 674 public override float PIDTau { set { m_PIDTau = value; } }
675 if (m_vehicle == null) 675
676 return (int)Vehicle.TYPE_NONE; 676 public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
677 else 677 public override bool PIDHoverActive { set { m_useHoverPID = value; } }
678 return (int)m_vehicle.Type; 678 public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
679 } 679 public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
680 set 680
681 { 681 public override Quaternion APIDTarget { set { return; } }
682 AddChange(changes.VehicleType, value); 682
683 } 683 public override bool APIDActive { set { return; } }
684 } 684
685 685 public override float APIDStrength { set { return; } }
686 public override void VehicleFloatParam(int param, float value) 686
687 { 687 public override float APIDDamping { set { return; } }
688 strVehicleFloatParam fp = new strVehicleFloatParam(); 688
689 fp.param = param; 689 public override int VehicleType
690 fp.value = value; 690 {
691 AddChange(changes.VehicleFloatParam, fp); 691 // we may need to put a fake on this
692 } 692 get
693 693 {
694 public override void VehicleVectorParam(int param, Vector3 value) 694 if (m_vehicle == null)
695 { 695 return (int)Vehicle.TYPE_NONE;
696 strVehicleVectorParam fp = new strVehicleVectorParam(); 696 else
697 fp.param = param; 697 return (int)m_vehicle.Type;
698 fp.value = value; 698 }
699 AddChange(changes.VehicleVectorParam, fp); 699 set
700 } 700 {
701 701 AddChange(changes.VehicleType, value);
702 public override void VehicleRotationParam(int param, Quaternion value) 702 }
703 { 703 }
704 strVehicleQuatParam fp = new strVehicleQuatParam(); 704
705 fp.param = param; 705 public override void VehicleFloatParam(int param, float value)
706 fp.value = value; 706 {
707 AddChange(changes.VehicleRotationParam, fp); 707 strVehicleFloatParam fp = new strVehicleFloatParam();
708 } 708 fp.param = param;
709 709 fp.value = value;
710 public override void VehicleFlags(int param, bool value) 710 AddChange(changes.VehicleFloatParam, fp);
711 { 711 }
712 strVehicleBoolParam bp = new strVehicleBoolParam(); 712
713 bp.param = param; 713 public override void VehicleVectorParam(int param, Vector3 value)
714 bp.value = value; 714 {
715 AddChange(changes.VehicleFlags, bp); 715 strVehicleVectorParam fp = new strVehicleVectorParam();
716 } 716 fp.param = param;
717 717 fp.value = value;
718 public override void SetVehicle(object vdata) 718 AddChange(changes.VehicleVectorParam, fp);
719 { 719 }
720 AddChange(changes.SetVehicle, vdata); 720
721 } 721 public override void VehicleRotationParam(int param, Quaternion value)
722 public void SetAcceleration(Vector3 accel) 722 {
723 { 723 strVehicleQuatParam fp = new strVehicleQuatParam();
724 _acceleration = accel; 724 fp.param = param;
725 } 725 fp.value = value;
726 726 AddChange(changes.VehicleRotationParam, fp);
727 public override void AddForce(Vector3 force, bool pushforce) 727 }
728 { 728
729 if (force.IsFinite()) 729 public override void VehicleFlags(int param, bool value)
730 { 730 {
731 AddChange(changes.AddForce, force / _parent_scene.ODE_STEPSIZE); 731 strVehicleBoolParam bp = new strVehicleBoolParam();
732 } 732 bp.param = param;
733 else 733 bp.value = value;
734 { 734 AddChange(changes.VehicleFlags, bp);
735 m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name); 735 }
736 } 736
737 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString()); 737 public override void SetVehicle(object vdata)
738 } 738 {
739 739 AddChange(changes.SetVehicle, vdata);
740 public override void AddAngularForce(Vector3 force, bool pushforce) 740 }
741 { 741 public void SetAcceleration(Vector3 accel)
742 if (force.IsFinite()) 742 {
743 { 743 _acceleration = accel;
744 AddChange(changes.AddAngForce, force / _parent_scene.ODE_STEPSIZE); 744 }
745 } 745
746 else 746 public override void AddForce(Vector3 force, bool pushforce)
747 { 747 {
748 m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name); 748 if (force.IsFinite())
749 } 749 {
750 } 750 AddChange(changes.AddForce, force / _parent_scene.ODE_STEPSIZE);
751 751 }
752 public override void CrossingFailure() 752 else
753 { 753 {
754 if (m_outbounds) 754 m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name);
755 { 755 }
756 _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f); 756 //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
757 _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f); 757 }
758 _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f); 758
759 759 public override void AddAngularForce(Vector3 force, bool pushforce)
760 m_lastposition = _position; 760 {
761 _velocity.X = 0; 761 if (force.IsFinite())
762 _velocity.Y = 0; 762 {
763 _velocity.Z = 0; 763 AddChange(changes.AddAngForce, force / _parent_scene.ODE_STEPSIZE);
764 764 }
765 m_lastVelocity = _velocity; 765 else
766 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) 766 {
767 m_vehicle.Stop(); 767 m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name);
768 768 }
769 if(Body != IntPtr.Zero) 769 }
770 d.BodySetLinearVel(Body, 0, 0, 0); // stop it 770
771 if (prim_geom != IntPtr.Zero) 771 public override void CrossingFailure()
772 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); 772 {
773 773 if (m_outbounds)
774 m_outbounds = false; 774 {
775 changeDisable(false); 775 _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f);
776 base.RequestPhysicsterseUpdate(); 776 _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f);
777 } 777 _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f);
778 } 778
779 779 m_lastposition = _position;
780 public override void SetMomentum(Vector3 momentum) 780 _velocity.X = 0;
781 { 781 _velocity.Y = 0;
782 } 782 _velocity.Z = 0;
783 783
784 public override void SetMaterial(int pMaterial) 784 m_lastVelocity = _velocity;
785 { 785 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
786 m_material = pMaterial; 786 m_vehicle.Stop();
787 mu = _parent_scene.m_materialContactsData[pMaterial].mu; 787
788 bounce = _parent_scene.m_materialContactsData[pMaterial].bounce; 788 if(Body != IntPtr.Zero)
789 } 789 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
790 790 if (prim_geom != IntPtr.Zero)
791 public void setPrimForRemoval() 791 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
792 { 792
793 AddChange(changes.Remove, null); 793 m_outbounds = false;
794 } 794 changeDisable(false);
795 795 base.RequestPhysicsterseUpdate();
796 public override void link(PhysicsActor obj) 796 }
797 { 797 }
798 AddChange(changes.Link, obj); 798
799 } 799 public override void SetMomentum(Vector3 momentum)
800 800 {
801 public override void delink() 801 }
802 { 802
803 AddChange(changes.DeLink, null); 803 public override void SetMaterial(int pMaterial)
804 } 804 {
805 805 m_material = pMaterial;
806 public override void LockAngularMotion(Vector3 axis) 806 mu = _parent_scene.m_materialContactsData[pMaterial].mu;
807 { 807 bounce = _parent_scene.m_materialContactsData[pMaterial].bounce;
808 // reverse the zero/non zero values for ODE. 808 }
809 if (axis.IsFinite()) 809
810 { 810 public void setPrimForRemoval()
811 axis.X = (axis.X > 0) ? 1f : 0f; 811 {
812 axis.Y = (axis.Y > 0) ? 1f : 0f; 812 AddChange(changes.Remove, null);
813 axis.Z = (axis.Z > 0) ? 1f : 0f; 813 }
814 m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z); 814
815 AddChange(changes.AngLock, axis); 815 public override void link(PhysicsActor obj)
816 } 816 {
817 else 817 AddChange(changes.Link, obj);
818 { 818 }
819 m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name); 819
820 } 820 public override void delink()
821 } 821 {
822 822 AddChange(changes.DeLink, null);
823 public override void SubscribeEvents(int ms) 823 }
824 { 824
825 m_eventsubscription = ms; 825 public override void LockAngularMotion(Vector3 axis)
826 _parent_scene.AddCollisionEventReporting(this); 826 {
827 } 827 // reverse the zero/non zero values for ODE.
828 828 if (axis.IsFinite())
829 public override void UnSubscribeEvents() 829 {
830 { 830 axis.X = (axis.X > 0) ? 1f : 0f;
831 _parent_scene.RemoveCollisionEventReporting(this); 831 axis.Y = (axis.Y > 0) ? 1f : 0f;
832 m_eventsubscription = 0; 832 axis.Z = (axis.Z > 0) ? 1f : 0f;
833 } 833 m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
834 834 AddChange(changes.AngLock, axis);
835 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact) 835 }
836 { 836 else
837 if (CollisionEventsThisFrame == null) 837 {
838 CollisionEventsThisFrame = new CollisionEventUpdate(); 838 m_log.WarnFormat("[PHYSICS]: Got NaN locking axis from Scene on Object {0}", Name);
839 839 }
840 CollisionEventsThisFrame.AddCollider(CollidedWith, contact); 840 }
841 } 841
842 842 public override void SubscribeEvents(int ms)
843 public void SendCollisions() 843 {
844 { 844 m_eventsubscription = ms;
845 if (CollisionEventsThisFrame == null) 845 _parent_scene.AddCollisionEventReporting(this);
846 return; 846 }
847 847
848 base.SendCollisionUpdate(CollisionEventsThisFrame); 848 public override void UnSubscribeEvents()
849 849 {
850 if (CollisionEventsThisFrame.m_objCollisionList.Count == 0) 850 _parent_scene.RemoveCollisionEventReporting(this);
851 CollisionEventsThisFrame = null; 851 m_eventsubscription = 0;
852 else 852 }
853 CollisionEventsThisFrame = new CollisionEventUpdate(); 853
854 } 854 public void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
855 855 {
856 public override bool SubscribedEvents() 856 if (CollisionEventsThisFrame == null)
857 { 857 CollisionEventsThisFrame = new CollisionEventUpdate();
858 if (m_eventsubscription > 0) 858
859 return true; 859 CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
860 return false; 860 }
861 } 861
862 862 public void SendCollisions()
863 863 {
864 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size, 864 if (CollisionEventsThisFrame == null)
865 Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,byte _shapeType,uint plocalID) 865 return;
866 { 866
867 Name = primName; 867 base.SendCollisionUpdate(CollisionEventsThisFrame);
868 LocalID = plocalID; 868
869 869 if (CollisionEventsThisFrame.m_objCollisionList.Count == 0)
870 m_vehicle = null; 870 CollisionEventsThisFrame = null;
871 871 else
872 if (!pos.IsFinite()) 872 CollisionEventsThisFrame = new CollisionEventUpdate();
873 { 873 }
874 pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f), 874
875 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f); 875 public override bool SubscribedEvents()
876 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name); 876 {
877 } 877 if (m_eventsubscription > 0)
878 _position = pos; 878 return true;
879 givefakepos = 0; 879 return false;
880 880 }
881 PID_D = parent_scene.bodyPIDD; 881
882 PID_G = parent_scene.bodyPIDG; 882
883 m_density = parent_scene.geomDefaultDensity; 883 public OdePrim(String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
884 // m_tensor = parent_scene.bodyMotorJointMaxforceTensor; 884 Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,byte _shapeType,uint plocalID)
885 body_autodisable_frames = parent_scene.bodyFramesAutoDisable; 885 {
886 886 Name = primName;
887 prim_geom = IntPtr.Zero; 887 LocalID = plocalID;
888 collide_geom = IntPtr.Zero; 888
889 Body = IntPtr.Zero; 889 m_vehicle = null;
890 890
891 if (!size.IsFinite()) 891 if (!pos.IsFinite())
892 { 892 {
893 size = new Vector3(0.5f, 0.5f, 0.5f); 893 pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
894 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name); 894 parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
895 } 895 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name);
896 896 }
897 if (size.X <= 0) size.X = 0.01f; 897 _position = pos;
898 if (size.Y <= 0) size.Y = 0.01f; 898 givefakepos = 0;
899 if (size.Z <= 0) size.Z = 0.01f; 899
900 900 PID_D = parent_scene.bodyPIDD;
901 _size = size; 901 PID_G = parent_scene.bodyPIDG;
902 902 m_density = parent_scene.geomDefaultDensity;
903 if (!QuaternionIsFinite(rotation)) 903 // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
904 { 904 body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
905 rotation = Quaternion.Identity; 905
906 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name); 906 prim_geom = IntPtr.Zero;
907 } 907 collide_geom = IntPtr.Zero;
908 908 Body = IntPtr.Zero;
909 _orientation = rotation; 909
910 givefakeori = 0; 910 if (!size.IsFinite())
911 911 {
912 _pbs = pbs; 912 size = new Vector3(0.5f, 0.5f, 0.5f);
913 913 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name);
914 _parent_scene = parent_scene; 914 }
915 m_targetSpace = IntPtr.Zero; 915
916 916 if (size.X <= 0) size.X = 0.01f;
917 if (pos.Z < 0) 917 if (size.Y <= 0) size.Y = 0.01f;
918 { 918 if (size.Z <= 0) size.Z = 0.01f;
919 m_isphysical = false; 919
920 } 920 _size = size;
921 else 921
922 { 922 if (!QuaternionIsFinite(rotation))
923 m_isphysical = pisPhysical; 923 {
924 } 924 rotation = Quaternion.Identity;
925 m_fakeisphysical = m_isphysical; 925 m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name);
926 926 }
927 m_isVolumeDetect = false; 927
928 928 _orientation = rotation;
929 m_force = Vector3.Zero; 929 givefakeori = 0;
930 930
931 m_iscolliding = false; 931 _pbs = pbs;
932 m_colliderfilter = 0; 932
933 m_softcolide = true; 933 _parent_scene = parent_scene;
934 m_NoColide = false; 934 m_targetSpace = IntPtr.Zero;
935 935
936 hasOOBoffsetFromMesh = false; 936 if (pos.Z < 0)
937 _triMeshData = IntPtr.Zero; 937 {
938 938 m_isphysical = false;
939 m_shapetype = _shapeType; 939 }
940 940 else
941 m_lastdoneSelected = false; 941 {
942 m_isSelected = false; 942 m_isphysical = pisPhysical;
943 m_delaySelect = false; 943 }
944 944 m_fakeisphysical = m_isphysical;
945 m_isphantom = pisPhantom; 945
946 m_fakeisphantom = pisPhantom; 946 m_isVolumeDetect = false;
947 947
948 mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu; 948 m_force = Vector3.Zero;
949 bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce; 949
950 950 m_iscolliding = false;
951 CalcPrimBodyData(); 951 m_colliderfilter = 0;
952 952 m_softcolide = true;
953 m_building = true; // control must set this to false when done 953 m_NoColide = false;
954 954
955 AddChange(changes.Add, null); 955 hasOOBoffsetFromMesh = false;
956 } 956 _triMeshData = IntPtr.Zero;
957 957
958 private void resetCollisionAccounting() 958 m_shapetype = _shapeType;
959 { 959
960 m_collisionscore = 0; 960 m_lastdoneSelected = false;
961 } 961 m_isSelected = false;
962 962 m_delaySelect = false;
963 private void createAMotor(Vector3 axis) 963
964 { 964 m_isphantom = pisPhantom;
965 if (Body == IntPtr.Zero) 965 m_fakeisphantom = pisPhantom;
966 return; 966
967 967 mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu;
968 if (Amotor != IntPtr.Zero) 968 bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce;
969 { 969
970 d.JointDestroy(Amotor); 970 CalcPrimBodyData();
971 Amotor = IntPtr.Zero; 971
972 } 972 m_mesh = null;
973 973 if (_parent_scene.needsMeshing(pbs))
974 int axisnum = 3 - (int)(axis.X + axis.Y + axis.Z); 974 {
975 975 bool convex;
976 if (axisnum <= 0) 976 if (m_shapetype == 0)
977 return; 977 convex = false;
978 978 else
979 // stop it 979 convex = true;
980 d.BodySetTorque(Body, 0, 0, 0); 980
981 d.BodySetAngularVel(Body, 0, 0, 0); 981 m_mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true, convex);
982 982 }
983 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero); 983
984 d.JointAttach(Amotor, Body, IntPtr.Zero); 984
985 985 m_building = true; // control must set this to false when done
986 d.JointSetAMotorMode(Amotor, 0); 986
987 987 AddChange(changes.Add, null);
988 d.JointSetAMotorNumAxes(Amotor, axisnum); 988 }
989 989
990 // get current orientation to lock 990 private void resetCollisionAccounting()
991 991 {
992 d.Quaternion dcur = d.BodyGetQuaternion(Body); 992 m_collisionscore = 0;
993 Quaternion curr; // crap convertion between identical things 993 }
994 curr.X = dcur.X; 994
995 curr.Y = dcur.Y; 995 private void createAMotor(Vector3 axis)
996 curr.Z = dcur.Z; 996 {
997 curr.W = dcur.W; 997 if (Body == IntPtr.Zero)
998 Vector3 ax; 998 return;
999 999
1000 int i = 0; 1000 if (Amotor != IntPtr.Zero)
1001 int j = 0; 1001 {
1002 if (axis.X == 0) 1002 d.JointDestroy(Amotor);
1003 { 1003 Amotor = IntPtr.Zero;
1004 ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X 1004 }
1005 // ODE should do this with axis relative to body 1 but seems to fail 1005
1006 d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z); 1006 int axisnum = 3 - (int)(axis.X + axis.Y + axis.Z);
1007 d.JointSetAMotorAngle(Amotor, 0, 0); 1007
1008 d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, -0.000001f); 1008 if (axisnum <= 0)
1009 d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0.000001f); 1009 return;
1010 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0); 1010
1011 d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f); 1011 // stop it
1012 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f); 1012 d.BodySetTorque(Body, 0, 0, 0);
1013 d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f); 1013 d.BodySetAngularVel(Body, 0, 0, 0);
1014 d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f); 1014
1015 d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f); 1015 Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
1016 i++; 1016 d.JointAttach(Amotor, Body, IntPtr.Zero);
1017 j = 256; // move to next axis set 1017
1018 } 1018 d.JointSetAMotorMode(Amotor, 0);
1019 1019
1020 if (axis.Y == 0) 1020 d.JointSetAMotorNumAxes(Amotor, axisnum);
1021 { 1021
1022 ax = (new Vector3(0, 1, 0)) * curr; 1022 // get current orientation to lock
1023 d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); 1023
1024 d.JointSetAMotorAngle(Amotor, i, 0); 1024 d.Quaternion dcur = d.BodyGetQuaternion(Body);
1025 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f); 1025 Quaternion curr; // crap convertion between identical things
1026 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f); 1026 curr.X = dcur.X;
1027 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); 1027 curr.Y = dcur.Y;
1028 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); 1028 curr.Z = dcur.Z;
1029 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); 1029 curr.W = dcur.W;
1030 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); 1030 Vector3 ax;
1031 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); 1031
1032 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); 1032 int i = 0;
1033 i++; 1033 int j = 0;
1034 j += 256; 1034 if (axis.X == 0)
1035 } 1035 {
1036 1036 ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X
1037 if (axis.Z == 0) 1037 // ODE should do this with axis relative to body 1 but seems to fail
1038 { 1038 d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z);
1039 ax = (new Vector3(0, 0, 1)) * curr; 1039 d.JointSetAMotorAngle(Amotor, 0, 0);
1040 d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z); 1040 d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, -0.000001f);
1041 d.JointSetAMotorAngle(Amotor, i, 0); 1041 d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0.000001f);
1042 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f); 1042 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0);
1043 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f); 1043 d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f);
1044 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0); 1044 d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f);
1045 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f); 1045 d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f);
1046 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f); 1046 d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f);
1047 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f); 1047 d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f);
1048 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f); 1048 i++;
1049 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f); 1049 j = 256; // move to next axis set
1050 } 1050 }
1051 } 1051
1052 1052 if (axis.Y == 0)
1053 private bool setMesh(OdeScene parent_scene) 1053 {
1054 { 1054 ax = (new Vector3(0, 1, 0)) * curr;
1055 if (Body != IntPtr.Zero) 1055 d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
1056 { 1056 d.JointSetAMotorAngle(Amotor, i, 0);
1057 if (childPrim) 1057 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f);
1058 { 1058 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f);
1059 if (_parent != null) 1059 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
1060 { 1060 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
1061 OdePrim parent = (OdePrim)_parent; 1061 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
1062 parent.ChildDelink(this, false); 1062 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
1063 } 1063 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
1064 } 1064 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
1065 else 1065 i++;
1066 { 1066 j += 256;
1067 DestroyBody(); 1067 }
1068 } 1068
1069 } 1069 if (axis.Z == 0)
1070 1070 {
1071 bool convex; 1071 ax = (new Vector3(0, 0, 1)) * curr;
1072 if (m_shapetype == 0) 1072 d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
1073 convex = false; 1073 d.JointSetAMotorAngle(Amotor, i, 0);
1074 else 1074 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, -0.000001f);
1075 convex = true; 1075 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0.000001f);
1076 1076 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
1077 IMesh mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true,convex); 1077 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
1078 if (mesh == null) 1078 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
1079 { 1079 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
1080 m_log.WarnFormat("[PHYSICS]: CreateMesh Failed on prim {0} at <{1},{2},{3}>.", Name, _position.X, _position.Y, _position.Z); 1080 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
1081 return false; 1081 d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
1082 } 1082 }
1083 1083 }
1084 IntPtr vertices, indices; 1084
1085 int vertexCount, indexCount; 1085 private bool setMesh(OdeScene parent_scene)
1086 int vertexStride, triStride; 1086 {
1087 1087 IntPtr vertices, indices;
1088 mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap 1088 int vertexCount, indexCount;
1089 mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage 1089 int vertexStride, triStride;
1090 1090
1091 if (vertexCount == 0 || indexCount == 0) 1091 if (Body != IntPtr.Zero)
1092 { 1092 {
1093 m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. mesh UUID {4}", 1093 if (childPrim)
1094 Name, _position.X, _position.Y, _position.Z, _pbs.SculptTexture.ToString()); 1094 {
1095 mesh.releaseSourceMeshData(); 1095 if (_parent != null)
1096 return false; 1096 {
1097 } 1097 OdePrim parent = (OdePrim)_parent;
1098 1098 parent.ChildDelink(this, false);
1099 primOOBoffset = mesh.GetCentroid(); 1099 }
1100 hasOOBoffsetFromMesh = true; 1100 }
1101 1101 else
1102 mesh.releaseSourceMeshData(); 1102 {
1103 1103 DestroyBody();
1104 IntPtr geo = IntPtr.Zero; 1104 }
1105 1105 }
1106 try 1106
1107 { 1107 IMesh mesh = null;
1108 _triMeshData = d.GeomTriMeshDataCreate(); 1108
1109 1109
1110 d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride); 1110 lock (m_meshlock)
1111 d.GeomTriMeshDataPreprocess(_triMeshData); 1111 {
1112 1112 if (m_mesh == null)
1113 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1113 {
1114 geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null); 1114 bool convex;
1115 } 1115 if (m_shapetype == 0)
1116 1116 convex = false;
1117 catch (Exception e) 1117 else
1118 { 1118 convex = true;
1119 m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e); 1119
1120 if (_triMeshData != IntPtr.Zero) 1120 mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, (int)LevelOfDetail.High, true, convex);
1121 { 1121 }
1122 d.GeomTriMeshDataDestroy(_triMeshData); 1122 else
1123 _triMeshData = IntPtr.Zero; 1123 {
1124 } 1124 mesh = m_mesh;
1125 return false; 1125 }
1126 } 1126
1127 1127 if (mesh == null)
1128 SetGeom(geo); 1128 {
1129 return true; 1129 m_log.WarnFormat("[PHYSICS]: CreateMesh Failed on prim {0} at <{1},{2},{3}>.", Name, _position.X, _position.Y, _position.Z);
1130 } 1130 return false;
1131 1131 }
1132 private void SetGeom(IntPtr geom) 1132
1133 { 1133
1134 prim_geom = geom; 1134 mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
1135 //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name); 1135 mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
1136 if (prim_geom != IntPtr.Zero) 1136
1137 { 1137 if (vertexCount == 0 || indexCount == 0)
1138 if (m_NoColide) 1138 {
1139 { 1139 m_log.WarnFormat("[PHYSICS]: Got invalid mesh on prim {0} at <{1},{2},{3}>. mesh UUID {4}",
1140 d.GeomSetCategoryBits(prim_geom, 0); 1140 Name, _position.X, _position.Y, _position.Z, _pbs.SculptTexture.ToString());
1141 if (m_isphysical) 1141 mesh.releaseSourceMeshData();
1142 { 1142 return false;
1143 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); 1143 }
1144 } 1144
1145 else 1145 primOOBoffset = mesh.GetCentroid();
1146 { 1146 hasOOBoffsetFromMesh = true;
1147 d.GeomSetCollideBits(prim_geom, 0); 1147
1148 d.GeomDisable(prim_geom); 1148 mesh.releaseSourceMeshData();
1149 } 1149 m_mesh = null;
1150 } 1150 }
1151 else 1151
1152 { 1152 IntPtr geo = IntPtr.Zero;
1153 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 1153
1154 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 1154 try
1155 } 1155 {
1156 1156 _triMeshData = d.GeomTriMeshDataCreate();
1157 CalcPrimBodyData(); 1157
1158 1158 d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
1159 _parent_scene.geom_name_map[prim_geom] = Name; 1159 d.GeomTriMeshDataPreprocess(_triMeshData);
1160 _parent_scene.actor_name_map[prim_geom] = this; 1160
1161 1161 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1162 } 1162 geo = d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null);
1163 else 1163 }
1164 m_log.Warn("Setting bad Geom"); 1164
1165 } 1165 catch (Exception e)
1166 1166 {
1167 1167 m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e);
1168 /// <summary> 1168 if (_triMeshData != IntPtr.Zero)
1169 /// Create a geometry for the given mesh in the given target space. 1169 {
1170 /// </summary> 1170 d.GeomTriMeshDataDestroy(_triMeshData);
1171 /// <param name="m_targetSpace"></param> 1171 _triMeshData = IntPtr.Zero;
1172 /// <param name="mesh">If null, then a mesh is used that is based on the profile shape data.</param> 1172 }
1173 private void CreateGeom() 1173 return false;
1174 { 1174 }
1175 if (_triMeshData != IntPtr.Zero) 1175
1176 { 1176 SetGeom(geo);
1177 d.GeomTriMeshDataDestroy(_triMeshData); 1177 return true;
1178 _triMeshData = IntPtr.Zero; 1178 }
1179 } 1179
1180 1180 private void SetGeom(IntPtr geom)
1181 bool haveMesh = false; 1181 {
1182 hasOOBoffsetFromMesh = false; 1182 prim_geom = geom;
1183 m_NoColide = false; 1183 //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name);
1184 1184 if (prim_geom != IntPtr.Zero)
1185 if (_parent_scene.needsMeshing(_pbs)) 1185 {
1186 { 1186 if (m_NoColide)
1187 haveMesh = setMesh(_parent_scene); // this will give a mesh to non trivial known prims 1187 {
1188 if (!haveMesh) 1188 d.GeomSetCategoryBits(prim_geom, 0);
1189 m_NoColide = true; 1189 if (m_isphysical)
1190 } 1190 {
1191 1191 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
1192 if (!haveMesh) 1192 }
1193 { 1193 else
1194 if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1 1194 {
1195 && _size.X == _size.Y && _size.Y == _size.Z) 1195 d.GeomSetCollideBits(prim_geom, 0);
1196 { // it's a sphere 1196 d.GeomDisable(prim_geom);
1197 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1197 }
1198 try 1198 }
1199 { 1199 else
1200 SetGeom(d.CreateSphere(m_targetSpace, _size.X * 0.5f)); 1200 {
1201 } 1201 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1202 catch (Exception e) 1202 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1203 { 1203 }
1204 m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e); 1204
1205 return; 1205 CalcPrimBodyData();
1206 } 1206
1207 } 1207 _parent_scene.geom_name_map[prim_geom] = Name;
1208 else 1208 _parent_scene.actor_name_map[prim_geom] = this;
1209 {// do it as a box 1209
1210 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1210 }
1211 try 1211 else
1212 { 1212 m_log.Warn("Setting bad Geom");
1213 //Console.WriteLine(" CreateGeom 4"); 1213 }
1214 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z)); 1214
1215 } 1215
1216 catch (Exception e) 1216 /// <summary>
1217 { 1217 /// Create a geometry for the given mesh in the given target space.
1218 m_log.Warn("[PHYSICS]: Create box failed: {0}", e); 1218 /// </summary>
1219 return; 1219 /// <param name="m_targetSpace"></param>
1220 } 1220 /// <param name="mesh">If null, then a mesh is used that is based on the profile shape data.</param>
1221 } 1221 private void CreateGeom()
1222 } 1222 {
1223 } 1223 if (_triMeshData != IntPtr.Zero)
1224 1224 {
1225 /// <summary> 1225 d.GeomTriMeshDataDestroy(_triMeshData);
1226 /// Set a new geometry for this prim. 1226 _triMeshData = IntPtr.Zero;
1227 /// </summary> 1227 }
1228 /// <param name="geom"></param> 1228
1229 private void RemoveGeom() 1229 bool haveMesh = false;
1230 { 1230 hasOOBoffsetFromMesh = false;
1231 if (prim_geom != IntPtr.Zero) 1231 m_NoColide = false;
1232 { 1232
1233 _parent_scene.geom_name_map.Remove(prim_geom); 1233 if (_parent_scene.needsMeshing(_pbs))
1234 _parent_scene.actor_name_map.Remove(prim_geom); 1234 {
1235 try 1235 haveMesh = setMesh(_parent_scene); // this will give a mesh to non trivial known prims
1236 { 1236 if (!haveMesh)
1237 d.GeomDestroy(prim_geom); 1237 m_NoColide = true;
1238 if (_triMeshData != IntPtr.Zero) 1238 }
1239 { 1239
1240 d.GeomTriMeshDataDestroy(_triMeshData); 1240 if (!haveMesh)
1241 _triMeshData = IntPtr.Zero; 1241 {
1242 } 1242 if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1
1243 } 1243 && _size.X == _size.Y && _size.Y == _size.Z)
1244 // catch (System.AccessViolationException) 1244 { // it's a sphere
1245 catch (Exception e) 1245 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1246 { 1246 try
1247 m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e); 1247 {
1248 } 1248 SetGeom(d.CreateSphere(m_targetSpace, _size.X * 0.5f));
1249 1249 }
1250 prim_geom = IntPtr.Zero; 1250 catch (Exception e)
1251 } 1251 {
1252 else 1252 m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e);
1253 { 1253 return;
1254 m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name); 1254 }
1255 } 1255 }
1256 Body = IntPtr.Zero; 1256 else
1257 hasOOBoffsetFromMesh = false; 1257 {// do it as a box
1258 CalcPrimBodyData(); 1258 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1259 } 1259 try
1260 1260 {
1261 private void ChildSetGeom(OdePrim odePrim) 1261 //Console.WriteLine(" CreateGeom 4");
1262 { 1262 SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
1263 // well.. 1263 }
1264 DestroyBody(); 1264 catch (Exception e)
1265 MakeBody(); 1265 {
1266 } 1266 m_log.Warn("[PHYSICS]: Create box failed: {0}", e);
1267 1267 return;
1268 //sets non physical prim m_targetSpace to right space in spaces grid for static prims 1268 }
1269 // should only be called for non physical prims unless they are becoming non physical 1269 }
1270 private void SetInStaticSpace(OdePrim prim) 1270 }
1271 { 1271 }
1272 IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace); 1272
1273 prim.m_targetSpace = targetSpace; 1273 /// <summary>
1274 d.GeomEnable(prim_geom); 1274 /// Set a new geometry for this prim.
1275 } 1275 /// </summary>
1276 1276 /// <param name="geom"></param>
1277 public void enableBodySoft() 1277 private void RemoveGeom()
1278 { 1278 {
1279 if (!childPrim && !m_isSelected) 1279 if (prim_geom != IntPtr.Zero)
1280 { 1280 {
1281 if (m_isphysical && Body != IntPtr.Zero) 1281 _parent_scene.geom_name_map.Remove(prim_geom);
1282 { 1282 _parent_scene.actor_name_map.Remove(prim_geom);
1283 if (m_isphantom && !m_isVolumeDetect) 1283 try
1284 { 1284 {
1285 m_collisionCategories = 0; 1285 d.GeomDestroy(prim_geom);
1286 m_collisionFlags = CollisionCategories.Land; 1286 if (_triMeshData != IntPtr.Zero)
1287 } 1287 {
1288 else 1288 d.GeomTriMeshDataDestroy(_triMeshData);
1289 { 1289 _triMeshData = IntPtr.Zero;
1290 m_collisionCategories |= CollisionCategories.Body; 1290 }
1291 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); 1291 }
1292 } 1292 // catch (System.AccessViolationException)
1293 1293 catch (Exception e)
1294 foreach (OdePrim prm in childrenPrim) 1294 {
1295 { 1295 m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e);
1296 prm.m_collisionCategories = m_collisionCategories; 1296 }
1297 prm.m_collisionFlags = m_collisionFlags; 1297
1298 1298 prim_geom = IntPtr.Zero;
1299 if (prm.prim_geom != IntPtr.Zero) 1299 }
1300 { 1300 else
1301 if (prm.m_NoColide) 1301 {
1302 { 1302 m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name);
1303 d.GeomSetCategoryBits(prm.prim_geom, 0); 1303 }
1304 d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); 1304 Body = IntPtr.Zero;
1305 } 1305 hasOOBoffsetFromMesh = false;
1306 else 1306 CalcPrimBodyData();
1307 { 1307 }
1308 d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); 1308
1309 d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); 1309 private void ChildSetGeom(OdePrim odePrim)
1310 } 1310 {
1311 d.GeomEnable(prm.prim_geom); 1311 // well..
1312 } 1312 DestroyBody();
1313 } 1313 MakeBody();
1314 1314 }
1315 if (prim_geom != IntPtr.Zero) 1315
1316 { 1316 //sets non physical prim m_targetSpace to right space in spaces grid for static prims
1317 if (m_NoColide) 1317 // should only be called for non physical prims unless they are becoming non physical
1318 { 1318 private void SetInStaticSpace(OdePrim prim)
1319 d.GeomSetCategoryBits(prim_geom, 0); 1319 {
1320 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); 1320 IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim._position, prim.m_targetSpace);
1321 } 1321 prim.m_targetSpace = targetSpace;
1322 else 1322 d.GeomEnable(prim_geom);
1323 { 1323 }
1324 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 1324
1325 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 1325 public void enableBodySoft()
1326 } 1326 {
1327 d.GeomEnable(prim_geom); 1327 if (!childPrim && !m_isSelected)
1328 } 1328 {
1329 d.BodyEnable(Body); 1329 if (m_isphysical && Body != IntPtr.Zero)
1330 } 1330 {
1331 } 1331 if (m_isphantom && !m_isVolumeDetect)
1332 m_disabled = false; 1332 {
1333 resetCollisionAccounting(); // this sets m_disable to false 1333 m_collisionCategories = 0;
1334 } 1334 m_collisionFlags = CollisionCategories.Land;
1335 1335 }
1336 private void disableBodySoft() 1336 else
1337 { 1337 {
1338 m_disabled = true; 1338 m_collisionCategories |= CollisionCategories.Body;
1339 if (!childPrim) 1339 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1340 { 1340 }
1341 if (m_isphysical && Body != IntPtr.Zero) 1341
1342 { 1342 foreach (OdePrim prm in childrenPrim)
1343 m_collisionCategories &= ~CollisionCategories.Body; 1343 {
1344 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); 1344 prm.m_collisionCategories = m_collisionCategories;
1345 1345 prm.m_collisionFlags = m_collisionFlags;
1346 foreach (OdePrim prm in childrenPrim) 1346
1347 { 1347 if (prm.prim_geom != IntPtr.Zero)
1348 prm.m_collisionCategories = m_collisionCategories; 1348 {
1349 prm.m_collisionFlags = m_collisionFlags; 1349 if (prm.m_NoColide)
1350 1350 {
1351 if (prm.prim_geom != IntPtr.Zero) 1351 d.GeomSetCategoryBits(prm.prim_geom, 0);
1352 { 1352 d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land);
1353 if (prm.m_NoColide) 1353 }
1354 { 1354 else
1355 d.GeomSetCategoryBits(prm.prim_geom, 0); 1355 {
1356 d.GeomSetCollideBits(prm.prim_geom, 0); 1356 d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories);
1357 } 1357 d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags);
1358 else 1358 }
1359 { 1359 d.GeomEnable(prm.prim_geom);
1360 d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); 1360 }
1361 d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); 1361 }
1362 } 1362
1363 d.GeomDisable(prm.prim_geom); 1363 if (prim_geom != IntPtr.Zero)
1364 } 1364 {
1365 } 1365 if (m_NoColide)
1366 1366 {
1367 if (prim_geom != IntPtr.Zero) 1367 d.GeomSetCategoryBits(prim_geom, 0);
1368 { 1368 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
1369 if (m_NoColide) 1369 }
1370 { 1370 else
1371 d.GeomSetCategoryBits(prim_geom, 0); 1371 {
1372 d.GeomSetCollideBits(prim_geom, 0); 1372 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1373 } 1373 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1374 else 1374 }
1375 { 1375 d.GeomEnable(prim_geom);
1376 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 1376 }
1377 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 1377 d.BodyEnable(Body);
1378 } 1378 }
1379 d.GeomDisable(prim_geom); 1379 }
1380 } 1380 m_disabled = false;
1381 1381 resetCollisionAccounting(); // this sets m_disable to false
1382 d.BodyDisable(Body); 1382 }
1383 } 1383
1384 } 1384 private void disableBodySoft()
1385 } 1385 {
1386 1386 m_disabled = true;
1387 private void MakeBody() 1387 if (!childPrim)
1388 { 1388 {
1389 if (!m_isphysical) // only physical get bodies 1389 if (m_isphysical && Body != IntPtr.Zero)
1390 return; 1390 {
1391 1391 m_collisionCategories &= ~CollisionCategories.Body;
1392 if (childPrim) // child prims don't get bodies; 1392 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
1393 return; 1393
1394 1394 foreach (OdePrim prm in childrenPrim)
1395 if (m_building) 1395 {
1396 return; 1396 prm.m_collisionCategories = m_collisionCategories;
1397 1397 prm.m_collisionFlags = m_collisionFlags;
1398 if (prim_geom == IntPtr.Zero) 1398
1399 { 1399 if (prm.prim_geom != IntPtr.Zero)
1400 m_log.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet"); 1400 {
1401 return; 1401 if (prm.m_NoColide)
1402 } 1402 {
1403 1403 d.GeomSetCategoryBits(prm.prim_geom, 0);
1404 if (Body != IntPtr.Zero) 1404 d.GeomSetCollideBits(prm.prim_geom, 0);
1405 { 1405 }
1406 d.BodyDestroy(Body); 1406 else
1407 Body = IntPtr.Zero; 1407 {
1408 m_log.Warn("[PHYSICS]: MakeBody called having a body"); 1408 d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories);
1409 } 1409 d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags);
1410 1410 }
1411 1411 d.GeomDisable(prm.prim_geom);
1412 if (d.GeomGetBody(prim_geom) != IntPtr.Zero) 1412 }
1413 { 1413 }
1414 d.GeomSetBody(prim_geom, IntPtr.Zero); 1414
1415 m_log.Warn("[PHYSICS]: MakeBody root geom already had a body"); 1415 if (prim_geom != IntPtr.Zero)
1416 } 1416 {
1417 1417 if (m_NoColide)
1418 d.Matrix3 mymat = new d.Matrix3(); 1418 {
1419 d.Quaternion myrot = new d.Quaternion(); 1419 d.GeomSetCategoryBits(prim_geom, 0);
1420 d.Mass objdmass = new d.Mass { }; 1420 d.GeomSetCollideBits(prim_geom, 0);
1421 1421 }
1422 Body = d.BodyCreate(_parent_scene.world); 1422 else
1423 1423 {
1424 DMassDup(ref primdMass, out objdmass); 1424 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1425 1425 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1426 // rotate inertia 1426 }
1427 myrot.X = _orientation.X; 1427 d.GeomDisable(prim_geom);
1428 myrot.Y = _orientation.Y; 1428 }
1429 myrot.Z = _orientation.Z; 1429
1430 myrot.W = _orientation.W; 1430 d.BodyDisable(Body);
1431 1431 }
1432 d.RfromQ(out mymat, ref myrot); 1432 }
1433 d.MassRotate(ref objdmass, ref mymat); 1433 }
1434 1434
1435 // set the body rotation 1435 private void MakeBody()
1436 d.BodySetRotation(Body, ref mymat); 1436 {
1437 1437 if (!m_isphysical) // only physical get bodies
1438 // recompute full object inertia if needed 1438 return;
1439 if (childrenPrim.Count > 0) 1439
1440 { 1440 if (childPrim) // child prims don't get bodies;
1441 d.Matrix3 mat = new d.Matrix3(); 1441 return;
1442 d.Quaternion quat = new d.Quaternion(); 1442
1443 d.Mass tmpdmass = new d.Mass { }; 1443 if (m_building)
1444 Vector3 rcm; 1444 return;
1445 1445
1446 rcm.X = _position.X + objdmass.c.X; 1446 if (prim_geom == IntPtr.Zero)
1447 rcm.Y = _position.Y + objdmass.c.Y; 1447 {
1448 rcm.Z = _position.Z + objdmass.c.Z; 1448 m_log.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet");
1449 1449 return;
1450 lock (childrenPrim) 1450 }
1451 { 1451
1452 foreach (OdePrim prm in childrenPrim) 1452 if (Body != IntPtr.Zero)
1453 { 1453 {
1454 if (prm.prim_geom == IntPtr.Zero) 1454 d.BodyDestroy(Body);
1455 { 1455 Body = IntPtr.Zero;
1456 m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet"); 1456 m_log.Warn("[PHYSICS]: MakeBody called having a body");
1457 continue; 1457 }
1458 } 1458
1459 1459
1460 DMassCopy(ref prm.primdMass, ref tmpdmass); 1460 if (d.GeomGetBody(prim_geom) != IntPtr.Zero)
1461 1461 {
1462 // apply prim current rotation to inertia 1462 d.GeomSetBody(prim_geom, IntPtr.Zero);
1463 quat.X = prm._orientation.X; 1463 m_log.Warn("[PHYSICS]: MakeBody root geom already had a body");
1464 quat.Y = prm._orientation.Y; 1464 }
1465 quat.Z = prm._orientation.Z; 1465
1466 quat.W = prm._orientation.W; 1466 d.Matrix3 mymat = new d.Matrix3();
1467 d.RfromQ(out mat, ref quat); 1467 d.Quaternion myrot = new d.Quaternion();
1468 d.MassRotate(ref tmpdmass, ref mat); 1468 d.Mass objdmass = new d.Mass { };
1469 1469
1470 Vector3 ppos = prm._position; 1470 Body = d.BodyCreate(_parent_scene.world);
1471 ppos.X += tmpdmass.c.X - rcm.X; 1471
1472 ppos.Y += tmpdmass.c.Y - rcm.Y; 1472 DMassDup(ref primdMass, out objdmass);
1473 ppos.Z += tmpdmass.c.Z - rcm.Z; 1473
1474 1474 // rotate inertia
1475 // refer inertia to root prim center of mass position 1475 myrot.X = _orientation.X;
1476 d.MassTranslate(ref tmpdmass, 1476 myrot.Y = _orientation.Y;
1477 ppos.X, 1477 myrot.Z = _orientation.Z;
1478 ppos.Y, 1478 myrot.W = _orientation.W;
1479 ppos.Z); 1479
1480 1480 d.RfromQ(out mymat, ref myrot);
1481 d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia 1481 d.MassRotate(ref objdmass, ref mymat);
1482 // fix prim colision cats 1482
1483 1483 // set the body rotation
1484 if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero) 1484 d.BodySetRotation(Body, ref mymat);
1485 { 1485
1486 d.GeomSetBody(prm.prim_geom, IntPtr.Zero); 1486 // recompute full object inertia if needed
1487 m_log.Warn("[PHYSICS]: MakeBody child geom already had a body"); 1487 if (childrenPrim.Count > 0)
1488 } 1488 {
1489 1489 d.Matrix3 mat = new d.Matrix3();
1490 d.GeomClearOffset(prm.prim_geom); 1490 d.Quaternion quat = new d.Quaternion();
1491 d.GeomSetBody(prm.prim_geom, Body); 1491 d.Mass tmpdmass = new d.Mass { };
1492 prm.Body = Body; 1492 Vector3 rcm;
1493 d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation 1493
1494 } 1494 rcm.X = _position.X + objdmass.c.X;
1495 } 1495 rcm.Y = _position.Y + objdmass.c.Y;
1496 } 1496 rcm.Z = _position.Z + objdmass.c.Z;
1497 1497
1498 d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset 1498 lock (childrenPrim)
1499 // associate root geom with body 1499 {
1500 d.GeomSetBody(prim_geom, Body); 1500 foreach (OdePrim prm in childrenPrim)
1501 1501 {
1502 d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z); 1502 if (prm.prim_geom == IntPtr.Zero)
1503 d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z); 1503 {
1504 1504 m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet");
1505 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body 1505 continue;
1506 myrot.W = -myrot.W; 1506 }
1507 d.RfromQ(out mymat, ref myrot); 1507
1508 d.MassRotate(ref objdmass, ref mymat); 1508 DMassCopy(ref prm.primdMass, ref tmpdmass);
1509 d.BodySetMass(Body, ref objdmass); 1509
1510 _mass = objdmass.mass; 1510 // apply prim current rotation to inertia
1511 1511 quat.X = prm._orientation.X;
1512 // disconnect from world gravity so we can apply buoyancy 1512 quat.Y = prm._orientation.Y;
1513 d.BodySetGravityMode(Body, false); 1513 quat.Z = prm._orientation.Z;
1514 1514 quat.W = prm._orientation.W;
1515 d.BodySetAutoDisableFlag(Body, true); 1515 d.RfromQ(out mat, ref quat);
1516 d.BodySetAutoDisableSteps(Body, body_autodisable_frames); 1516 d.MassRotate(ref tmpdmass, ref mat);
1517 // d.BodySetLinearDampingThreshold(Body, 0.01f); 1517
1518 // d.BodySetAngularDampingThreshold(Body, 0.001f); 1518 Vector3 ppos = prm._position;
1519 d.BodySetDamping(Body, .002f, .002f); 1519 ppos.X += tmpdmass.c.X - rcm.X;
1520 1520 ppos.Y += tmpdmass.c.Y - rcm.Y;
1521 1521 ppos.Z += tmpdmass.c.Z - rcm.Z;
1522 if (m_targetSpace != IntPtr.Zero) 1522
1523 { 1523 // refer inertia to root prim center of mass position
1524 _parent_scene.waitForSpaceUnlock(m_targetSpace); 1524 d.MassTranslate(ref tmpdmass,
1525 if (d.SpaceQuery(m_targetSpace, prim_geom)) 1525 ppos.X,
1526 d.SpaceRemove(m_targetSpace, prim_geom); 1526 ppos.Y,
1527 } 1527 ppos.Z);
1528 1528
1529 1529 d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia
1530 if (childrenPrim.Count == 0) 1530 // fix prim colision cats
1531 { 1531
1532 collide_geom = prim_geom; 1532 if (d.GeomGetBody(prm.prim_geom) != IntPtr.Zero)
1533 m_targetSpace = _parent_scene.ActiveSpace; 1533 {
1534 d.SpaceAdd(m_targetSpace, prim_geom); 1534 d.GeomSetBody(prm.prim_geom, IntPtr.Zero);
1535 } 1535 m_log.Warn("[PHYSICS]: MakeBody child geom already had a body");
1536 else 1536 }
1537 { 1537
1538 m_targetSpace = d.HashSpaceCreate(_parent_scene.ActiveSpace); 1538 d.GeomClearOffset(prm.prim_geom);
1539 d.HashSpaceSetLevels(m_targetSpace, -2, 8); 1539 d.GeomSetBody(prm.prim_geom, Body);
1540 d.SpaceSetSublevel(m_targetSpace, 3); 1540 prm.Body = Body;
1541 d.SpaceSetCleanup(m_targetSpace, false); 1541 d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation
1542 d.SpaceAdd(m_targetSpace, prim_geom); 1542 }
1543 collide_geom = m_targetSpace; 1543 }
1544 } 1544 }
1545 1545
1546 if (m_delaySelect) 1546 d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset
1547 { 1547 // associate root geom with body
1548 m_isSelected = true; 1548 d.GeomSetBody(prim_geom, Body);
1549 m_delaySelect = false; 1549
1550 } 1550 d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z);
1551 1551 d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z);
1552 lock (childrenPrim) 1552
1553 { 1553 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
1554 foreach (OdePrim prm in childrenPrim) 1554 myrot.W = -myrot.W;
1555 { 1555 d.RfromQ(out mymat, ref myrot);
1556 if (prm.prim_geom == IntPtr.Zero) 1556 d.MassRotate(ref objdmass, ref mymat);
1557 continue; 1557 d.BodySetMass(Body, ref objdmass);
1558 1558 _mass = objdmass.mass;
1559 Vector3 ppos = prm._position; 1559
1560 d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position 1560 // disconnect from world gravity so we can apply buoyancy
1561 1561 d.BodySetGravityMode(Body, false);
1562 if (prm.m_targetSpace != m_targetSpace) 1562
1563 { 1563 d.BodySetAutoDisableFlag(Body, true);
1564 if (prm.m_targetSpace != IntPtr.Zero) 1564 d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
1565 { 1565 // d.BodySetLinearDampingThreshold(Body, 0.01f);
1566 _parent_scene.waitForSpaceUnlock(prm.m_targetSpace); 1566 // d.BodySetAngularDampingThreshold(Body, 0.001f);
1567 if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom)) 1567 d.BodySetDamping(Body, .002f, .002f);
1568 d.SpaceRemove(prm.m_targetSpace, prm.prim_geom); 1568
1569 } 1569
1570 prm.m_targetSpace = m_targetSpace; 1570 if (m_targetSpace != IntPtr.Zero)
1571 d.SpaceAdd(m_targetSpace, prm.prim_geom); 1571 {
1572 } 1572 _parent_scene.waitForSpaceUnlock(m_targetSpace);
1573 1573 if (d.SpaceQuery(m_targetSpace, prim_geom))
1574 if (m_isSelected || m_disabled) 1574 d.SpaceRemove(m_targetSpace, prim_geom);
1575 { 1575 }
1576 prm.m_collisionCategories &= ~CollisionCategories.Body; 1576
1577 prm.m_collisionFlags &= ~(CollisionCategories.Land | CollisionCategories.Wind); 1577
1578 d.GeomDisable(prm.prim_geom); 1578 if (childrenPrim.Count == 0)
1579 } 1579 {
1580 else 1580 collide_geom = prim_geom;
1581 { 1581 m_targetSpace = _parent_scene.ActiveSpace;
1582 if (m_isphantom && !m_isVolumeDetect) 1582 d.SpaceAdd(m_targetSpace, prim_geom);
1583 { 1583 }
1584 prm.m_collisionCategories = 0; 1584 else
1585 prm.m_collisionFlags = CollisionCategories.Land; 1585 {
1586 } 1586 m_targetSpace = d.HashSpaceCreate(_parent_scene.ActiveSpace);
1587 else 1587 d.HashSpaceSetLevels(m_targetSpace, -2, 8);
1588 { 1588 d.SpaceSetSublevel(m_targetSpace, 3);
1589 prm.m_collisionCategories |= CollisionCategories.Body; 1589 d.SpaceSetCleanup(m_targetSpace, false);
1590 prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); 1590 d.SpaceAdd(m_targetSpace, prim_geom);
1591 } 1591 collide_geom = m_targetSpace;
1592 d.GeomEnable(prm.prim_geom); 1592 }
1593 } 1593
1594 1594 if (m_delaySelect)
1595 if (prm.m_NoColide) 1595 {
1596 { 1596 m_isSelected = true;
1597 d.GeomSetCategoryBits(prm.prim_geom, 0); 1597 m_delaySelect = false;
1598 d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); 1598 }
1599 d.GeomEnable(prm.prim_geom); 1599
1600 } 1600 lock (childrenPrim)
1601 else 1601 {
1602 { 1602 foreach (OdePrim prm in childrenPrim)
1603 d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories); 1603 {
1604 d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags); 1604 if (prm.prim_geom == IntPtr.Zero)
1605 } 1605 continue;
1606 prm.m_collisionscore = 0; 1606
1607 1607 Vector3 ppos = prm._position;
1608 if(!m_disabled) 1608 d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position
1609 prm.m_disabled = false; 1609
1610 1610 if (prm.m_targetSpace != m_targetSpace)
1611 _parent_scene.addActivePrim(prm); 1611 {
1612 } 1612 if (prm.m_targetSpace != IntPtr.Zero)
1613 } 1613 {
1614 1614 _parent_scene.waitForSpaceUnlock(prm.m_targetSpace);
1615 // The body doesn't already have a finite rotation mode set here 1615 if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom))
1616 if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null) 1616 d.SpaceRemove(prm.m_targetSpace, prm.prim_geom);
1617 { 1617 }
1618 createAMotor(m_angularlock); 1618 prm.m_targetSpace = m_targetSpace;
1619 } 1619 d.SpaceAdd(m_targetSpace, prm.prim_geom);
1620 1620 }
1621 if (m_isSelected || m_disabled) 1621
1622 { 1622 if (m_isSelected || m_disabled)
1623 m_collisionCategories &= ~CollisionCategories.Body; 1623 {
1624 m_collisionFlags &= ~(CollisionCategories.Land | CollisionCategories.Wind); 1624 prm.m_collisionCategories &= ~CollisionCategories.Body;
1625 1625 prm.m_collisionFlags &= ~(CollisionCategories.Land | CollisionCategories.Wind);
1626 d.GeomDisable(prim_geom); 1626 d.GeomDisable(prm.prim_geom);
1627 d.BodyDisable(Body); 1627 }
1628 } 1628 else
1629 else 1629 {
1630 { 1630 if (m_isphantom && !m_isVolumeDetect)
1631 if (m_isphantom && !m_isVolumeDetect) 1631 {
1632 { 1632 prm.m_collisionCategories = 0;
1633 m_collisionCategories = 0; 1633 prm.m_collisionFlags = CollisionCategories.Land;
1634 m_collisionFlags = CollisionCategories.Land; 1634 }
1635 } 1635 else
1636 else 1636 {
1637 { 1637 prm.m_collisionCategories |= CollisionCategories.Body;
1638 m_collisionCategories |= CollisionCategories.Body; 1638 prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1639 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind); 1639 }
1640 } 1640 d.GeomEnable(prm.prim_geom);
1641 1641 }
1642 d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z); 1642
1643 d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z); 1643 if (prm.m_NoColide)
1644 } 1644 {
1645 1645 d.GeomSetCategoryBits(prm.prim_geom, 0);
1646 if (m_NoColide) 1646 d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land);
1647 { 1647 d.GeomEnable(prm.prim_geom);
1648 d.GeomSetCategoryBits(prim_geom, 0); 1648 }
1649 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); 1649 else
1650 } 1650 {
1651 else 1651 d.GeomSetCategoryBits(prm.prim_geom, (int)prm.m_collisionCategories);
1652 { 1652 d.GeomSetCollideBits(prm.prim_geom, (int)prm.m_collisionFlags);
1653 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 1653 }
1654 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 1654 prm.m_collisionscore = 0;
1655 } 1655
1656 1656 if(!m_disabled)
1657 m_collisionscore = 0; 1657 prm.m_disabled = false;
1658 1658
1659 m_softcolide = true; 1659 _parent_scene.addActivePrim(prm);
1660 _parent_scene.addActivePrim(this); 1660 }
1661 _parent_scene.addActiveGroups(this); 1661 }
1662 } 1662
1663 1663 // The body doesn't already have a finite rotation mode set here
1664 private void DestroyBody() 1664 if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null)
1665 { 1665 {
1666 if (Body != IntPtr.Zero) 1666 createAMotor(m_angularlock);
1667 { 1667 }
1668 _parent_scene.remActivePrim(this); 1668
1669 m_collisionCategories &= ~CollisionCategories.Body; 1669 if (m_isSelected || m_disabled)
1670 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land); 1670 {
1671 if (prim_geom != IntPtr.Zero) 1671 m_collisionCategories &= ~CollisionCategories.Body;
1672 { 1672 m_collisionFlags &= ~(CollisionCategories.Land | CollisionCategories.Wind);
1673 if (m_NoColide) 1673
1674 { 1674 d.GeomDisable(prim_geom);
1675 d.GeomSetCategoryBits(prim_geom, 0); 1675 d.BodyDisable(Body);
1676 d.GeomSetCollideBits(prim_geom, 0); 1676 }
1677 } 1677 else
1678 else 1678 {
1679 { 1679 if (m_isphantom && !m_isVolumeDetect)
1680 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 1680 {
1681 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 1681 m_collisionCategories = 0;
1682 } 1682 m_collisionFlags = CollisionCategories.Land;
1683 UpdateDataFromGeom(); 1683 }
1684 d.GeomSetBody(prim_geom, IntPtr.Zero); 1684 else
1685 SetInStaticSpace(this); 1685 {
1686 } 1686 m_collisionCategories |= CollisionCategories.Body;
1687 1687 m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
1688 if (!childPrim) 1688 }
1689 { 1689
1690 lock (childrenPrim) 1690 d.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z);
1691 { 1691 d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
1692 foreach (OdePrim prm in childrenPrim) 1692 }
1693 { 1693
1694 _parent_scene.remActivePrim(prm); 1694 if (m_NoColide)
1695 prm.m_collisionCategories = m_collisionCategories; 1695 {
1696 prm.m_collisionFlags = m_collisionFlags; 1696 d.GeomSetCategoryBits(prim_geom, 0);
1697 if (prm.prim_geom != IntPtr.Zero) 1697 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
1698 { 1698 }
1699 if (prm.m_NoColide) 1699 else
1700 { 1700 {
1701 d.GeomSetCategoryBits(prm.prim_geom, 0); 1701 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1702 d.GeomSetCollideBits(prm.prim_geom, 0); 1702 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1703 } 1703 }
1704 else 1704
1705 { 1705 m_collisionscore = 0;
1706 d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); 1706
1707 d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); 1707 m_softcolide = true;
1708 } 1708 _parent_scene.addActivePrim(this);
1709 prm.UpdateDataFromGeom(); 1709 _parent_scene.addActiveGroups(this);
1710 SetInStaticSpace(prm); 1710 }
1711 } 1711
1712 prm.Body = IntPtr.Zero; 1712 private void DestroyBody()
1713 prm._mass = prm.primMass; 1713 {
1714 prm.m_collisionscore = 0; 1714 if (Body != IntPtr.Zero)
1715 } 1715 {
1716 } 1716 _parent_scene.remActivePrim(this);
1717 if (Amotor != IntPtr.Zero) 1717 m_collisionCategories &= ~CollisionCategories.Body;
1718 { 1718 m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
1719 d.JointDestroy(Amotor); 1719 if (prim_geom != IntPtr.Zero)
1720 Amotor = IntPtr.Zero; 1720 {
1721 } 1721 if (m_NoColide)
1722 _parent_scene.remActiveGroup(this); 1722 {
1723 d.BodyDestroy(Body); 1723 d.GeomSetCategoryBits(prim_geom, 0);
1724 } 1724 d.GeomSetCollideBits(prim_geom, 0);
1725 Body = IntPtr.Zero; 1725 }
1726 } 1726 else
1727 _mass = primMass; 1727 {
1728 m_collisionscore = 0; 1728 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
1729 } 1729 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
1730 1730 }
1731 private void FixInertia(Vector3 NewPos,Quaternion newrot) 1731 UpdateDataFromGeom();
1732 { 1732 d.GeomSetBody(prim_geom, IntPtr.Zero);
1733 d.Matrix3 mat = new d.Matrix3(); 1733 SetInStaticSpace(this);
1734 d.Quaternion quat = new d.Quaternion(); 1734 }
1735 1735
1736 d.Mass tmpdmass = new d.Mass { }; 1736 if (!childPrim)
1737 d.Mass objdmass = new d.Mass { }; 1737 {
1738 1738 lock (childrenPrim)
1739 d.BodyGetMass(Body, out tmpdmass); 1739 {
1740 objdmass = tmpdmass; 1740 foreach (OdePrim prm in childrenPrim)
1741 1741 {
1742 d.Vector3 dobjpos; 1742 _parent_scene.remActivePrim(prm);
1743 d.Vector3 thispos; 1743 prm.m_collisionCategories = m_collisionCategories;
1744 1744 prm.m_collisionFlags = m_collisionFlags;
1745 // get current object position and rotation 1745 if (prm.prim_geom != IntPtr.Zero)
1746 dobjpos = d.BodyGetPosition(Body); 1746 {
1747 1747 if (prm.m_NoColide)
1748 // get prim own inertia in its local frame 1748 {
1749 tmpdmass = primdMass; 1749 d.GeomSetCategoryBits(prm.prim_geom, 0);
1750 1750 d.GeomSetCollideBits(prm.prim_geom, 0);
1751 // transform to object frame 1751 }
1752 mat = d.GeomGetOffsetRotation(prim_geom); 1752 else
1753 d.MassRotate(ref tmpdmass, ref mat); 1753 {
1754 1754 d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories);
1755 thispos = d.GeomGetOffsetPosition(prim_geom); 1755 d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags);
1756 d.MassTranslate(ref tmpdmass, 1756 }
1757 thispos.X, 1757 prm.UpdateDataFromGeom();
1758 thispos.Y, 1758 SetInStaticSpace(prm);
1759 thispos.Z); 1759 }
1760 1760 prm.Body = IntPtr.Zero;
1761 // subtract current prim inertia from object 1761 prm._mass = prm.primMass;
1762 DMassSubPartFromObj(ref tmpdmass, ref objdmass); 1762 prm.m_collisionscore = 0;
1763 1763 }
1764 // back prim own inertia 1764 }
1765 tmpdmass = primdMass; 1765 if (Amotor != IntPtr.Zero)
1766 1766 {
1767 // update to new position and orientation 1767 d.JointDestroy(Amotor);
1768 _position = NewPos; 1768 Amotor = IntPtr.Zero;
1769 d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z); 1769 }
1770 _orientation = newrot; 1770 _parent_scene.remActiveGroup(this);
1771 quat.X = newrot.X; 1771 d.BodyDestroy(Body);
1772 quat.Y = newrot.Y; 1772 }
1773 quat.Z = newrot.Z; 1773 Body = IntPtr.Zero;
1774 quat.W = newrot.W; 1774 }
1775 d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat); 1775 _mass = primMass;
1776 1776 m_collisionscore = 0;
1777 mat = d.GeomGetOffsetRotation(prim_geom); 1777 }
1778 d.MassRotate(ref tmpdmass, ref mat); 1778
1779 1779 private void FixInertia(Vector3 NewPos,Quaternion newrot)
1780 thispos = d.GeomGetOffsetPosition(prim_geom); 1780 {
1781 d.MassTranslate(ref tmpdmass, 1781 d.Matrix3 mat = new d.Matrix3();
1782 thispos.X, 1782 d.Quaternion quat = new d.Quaternion();
1783 thispos.Y, 1783
1784 thispos.Z); 1784 d.Mass tmpdmass = new d.Mass { };
1785 1785 d.Mass objdmass = new d.Mass { };
1786 d.MassAdd(ref objdmass, ref tmpdmass); 1786
1787 1787 d.BodyGetMass(Body, out tmpdmass);
1788 // fix all positions 1788 objdmass = tmpdmass;
1789 IntPtr g = d.BodyGetFirstGeom(Body); 1789
1790 while (g != IntPtr.Zero) 1790 d.Vector3 dobjpos;
1791 { 1791 d.Vector3 thispos;
1792 thispos = d.GeomGetOffsetPosition(g); 1792
1793 thispos.X -= objdmass.c.X; 1793 // get current object position and rotation
1794 thispos.Y -= objdmass.c.Y; 1794 dobjpos = d.BodyGetPosition(Body);
1795 thispos.Z -= objdmass.c.Z; 1795
1796 d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z); 1796 // get prim own inertia in its local frame
1797 g = d.dBodyGetNextGeom(g); 1797 tmpdmass = primdMass;
1798
1799 // transform to object frame
1800 mat = d.GeomGetOffsetRotation(prim_geom);
1801 d.MassRotate(ref tmpdmass, ref mat);
1802
1803 thispos = d.GeomGetOffsetPosition(prim_geom);
1804 d.MassTranslate(ref tmpdmass,
1805 thispos.X,
1806 thispos.Y,
1807 thispos.Z);
1808
1809 // subtract current prim inertia from object
1810 DMassSubPartFromObj(ref tmpdmass, ref objdmass);
1811
1812 // back prim own inertia
1813 tmpdmass = primdMass;
1814
1815 // update to new position and orientation
1816 _position = NewPos;
1817 d.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z);
1818 _orientation = newrot;
1819 quat.X = newrot.X;
1820 quat.Y = newrot.Y;
1821 quat.Z = newrot.Z;
1822 quat.W = newrot.W;
1823 d.GeomSetOffsetWorldQuaternion(prim_geom, ref quat);
1824
1825 mat = d.GeomGetOffsetRotation(prim_geom);
1826 d.MassRotate(ref tmpdmass, ref mat);
1827
1828 thispos = d.GeomGetOffsetPosition(prim_geom);
1829 d.MassTranslate(ref tmpdmass,
1830 thispos.X,
1831 thispos.Y,
1832 thispos.Z);
1833
1834 d.MassAdd(ref objdmass, ref tmpdmass);
1835
1836 // fix all positions
1837 IntPtr g = d.BodyGetFirstGeom(Body);
1838 while (g != IntPtr.Zero)
1839 {
1840 thispos = d.GeomGetOffsetPosition(g);
1841 thispos.X -= objdmass.c.X;
1842 thispos.Y -= objdmass.c.Y;
1843 thispos.Z -= objdmass.c.Z;
1844 d.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
1845 g = d.dBodyGetNextGeom(g);
1798 } 1846 }
1799 d.BodyVectorToWorld(Body,objdmass.c.X, objdmass.c.Y, objdmass.c.Z,out thispos); 1847 d.BodyVectorToWorld(Body,objdmass.c.X, objdmass.c.Y, objdmass.c.Z,out thispos);
1800 1848
1801 d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z); 1849 d.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
1802 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body 1850 d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
1803 d.BodySetMass(Body, ref objdmass); 1851 d.BodySetMass(Body, ref objdmass);
1804 _mass = objdmass.mass; 1852 _mass = objdmass.mass;
1805 } 1853 }
1806 1854
1807 1855
@@ -1935,2042 +1983,2042 @@ namespace OpenSim.Region.Physics.OdePlugin
1935 d.BodySetMass(Body, ref objdmass); 1983 d.BodySetMass(Body, ref objdmass);
1936 _mass = objdmass.mass; 1984 _mass = objdmass.mass;
1937 } 1985 }
1938 1986
1939 1987
1940 #region Mass Calculation 1988 #region Mass Calculation
1941 1989
1942 private float CalculatePrimVolume() 1990 private float CalculatePrimVolume()
1943 { 1991 {
1944 float volume = _size.X * _size.Y * _size.Z; // default 1992 float volume = _size.X * _size.Y * _size.Z; // default
1945 float tmp; 1993 float tmp;
1946 1994
1947 float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; 1995 float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f;
1948 float hollowVolume = hollowAmount * hollowAmount; 1996 float hollowVolume = hollowAmount * hollowAmount;
1949 1997
1950 switch (_pbs.ProfileShape) 1998 switch (_pbs.ProfileShape)
1951 { 1999 {
1952 case ProfileShape.Square: 2000 case ProfileShape.Square:
1953 // default box 2001 // default box
1954 2002
1955 if (_pbs.PathCurve == (byte)Extrusion.Straight) 2003 if (_pbs.PathCurve == (byte)Extrusion.Straight)
1956 { 2004 {
1957 if (hollowAmount > 0.0) 2005 if (hollowAmount > 0.0)
1958 { 2006 {
1959 switch (_pbs.HollowShape) 2007 switch (_pbs.HollowShape)
1960 { 2008 {
1961 case HollowShape.Square: 2009 case HollowShape.Square:
1962 case HollowShape.Same: 2010 case HollowShape.Same:
1963 break; 2011 break;
1964 2012
1965 case HollowShape.Circle: 2013 case HollowShape.Circle:
1966 2014
1967 hollowVolume *= 0.78539816339f; 2015 hollowVolume *= 0.78539816339f;
1968 break; 2016 break;
1969 2017
1970 case HollowShape.Triangle: 2018 case HollowShape.Triangle:
1971 2019
1972 hollowVolume *= (0.5f * .5f); 2020 hollowVolume *= (0.5f * .5f);
1973 break; 2021 break;
1974 2022
1975 default: 2023 default:
1976 hollowVolume = 0; 2024 hollowVolume = 0;
1977 break; 2025 break;
1978 } 2026 }
1979 volume *= (1.0f - hollowVolume); 2027 volume *= (1.0f - hollowVolume);
1980 } 2028 }
1981 } 2029 }
1982 2030
1983 else if (_pbs.PathCurve == (byte)Extrusion.Curve1) 2031 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
1984 { 2032 {
1985 //a tube 2033 //a tube
1986 2034
1987 volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); 2035 volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX);
1988 tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY); 2036 tmp = 1.0f - 2.0e-2f * (float)(200 - _pbs.PathScaleY);
1989 volume -= volume * tmp * tmp; 2037 volume -= volume * tmp * tmp;
1990 2038
1991 if (hollowAmount > 0.0) 2039 if (hollowAmount > 0.0)
1992 { 2040 {
1993 hollowVolume *= hollowAmount; 2041 hollowVolume *= hollowAmount;
1994 2042
1995 switch (_pbs.HollowShape) 2043 switch (_pbs.HollowShape)
1996 { 2044 {
1997 case HollowShape.Square: 2045 case HollowShape.Square:
1998 case HollowShape.Same: 2046 case HollowShape.Same:
1999 break; 2047 break;
2000 2048
2001 case HollowShape.Circle: 2049 case HollowShape.Circle:
2002 hollowVolume *= 0.78539816339f; 2050 hollowVolume *= 0.78539816339f;
2003 break; 2051 break;
2004 2052
2005 case HollowShape.Triangle: 2053 case HollowShape.Triangle:
2006 hollowVolume *= 0.5f * 0.5f; 2054 hollowVolume *= 0.5f * 0.5f;
2007 break; 2055 break;
2008 default: 2056 default:
2009 hollowVolume = 0; 2057 hollowVolume = 0;
2010 break; 2058 break;
2011 } 2059 }
2012 volume *= (1.0f - hollowVolume); 2060 volume *= (1.0f - hollowVolume);
2013 } 2061 }
2014 } 2062 }
2015 2063
2016 break; 2064 break;
2017 2065
2018 case ProfileShape.Circle: 2066 case ProfileShape.Circle:
2019 2067
2020 if (_pbs.PathCurve == (byte)Extrusion.Straight) 2068 if (_pbs.PathCurve == (byte)Extrusion.Straight)
2021 { 2069 {
2022 volume *= 0.78539816339f; // elipse base 2070 volume *= 0.78539816339f; // elipse base
2023 2071
2024 if (hollowAmount > 0.0) 2072 if (hollowAmount > 0.0)
2025 { 2073 {
2026 switch (_pbs.HollowShape) 2074 switch (_pbs.HollowShape)
2027 { 2075 {
2028 case HollowShape.Same: 2076 case HollowShape.Same:
2029 case HollowShape.Circle: 2077 case HollowShape.Circle:
2030 break; 2078 break;
2031 2079
2032 case HollowShape.Square: 2080 case HollowShape.Square:
2033 hollowVolume *= 0.5f * 2.5984480504799f; 2081 hollowVolume *= 0.5f * 2.5984480504799f;
2034 break; 2082 break;
2035 2083
2036 case HollowShape.Triangle: 2084 case HollowShape.Triangle:
2037 hollowVolume *= .5f * 1.27323954473516f; 2085 hollowVolume *= .5f * 1.27323954473516f;
2038 break; 2086 break;
2039 2087
2040 default: 2088 default:
2041 hollowVolume = 0; 2089 hollowVolume = 0;
2042 break; 2090 break;
2043 } 2091 }
2044 volume *= (1.0f - hollowVolume); 2092 volume *= (1.0f - hollowVolume);
2045 } 2093 }
2046 } 2094 }
2047 2095
2048 else if (_pbs.PathCurve == (byte)Extrusion.Curve1) 2096 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
2049 { 2097 {
2050 volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); 2098 volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX);
2051 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); 2099 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
2052 volume *= (1.0f - tmp * tmp); 2100 volume *= (1.0f - tmp * tmp);
2053 2101
2054 if (hollowAmount > 0.0) 2102 if (hollowAmount > 0.0)
2055 { 2103 {
2056 2104
2057 // calculate the hollow volume by it's shape compared to the prim shape 2105 // calculate the hollow volume by it's shape compared to the prim shape
2058 hollowVolume *= hollowAmount; 2106 hollowVolume *= hollowAmount;
2059 2107
2060 switch (_pbs.HollowShape) 2108 switch (_pbs.HollowShape)
2061 { 2109 {
2062 case HollowShape.Same: 2110 case HollowShape.Same:
2063 case HollowShape.Circle: 2111 case HollowShape.Circle:
2064 break; 2112 break;
2065 2113
2066 case HollowShape.Square: 2114 case HollowShape.Square:
2067 hollowVolume *= 0.5f * 2.5984480504799f; 2115 hollowVolume *= 0.5f * 2.5984480504799f;
2068 break; 2116 break;
2069 2117
2070 case HollowShape.Triangle: 2118 case HollowShape.Triangle:
2071 hollowVolume *= .5f * 1.27323954473516f; 2119 hollowVolume *= .5f * 1.27323954473516f;
2072 break; 2120 break;
2073 2121
2074 default: 2122 default:
2075 hollowVolume = 0; 2123 hollowVolume = 0;
2076 break; 2124 break;
2077 } 2125 }
2078 volume *= (1.0f - hollowVolume); 2126 volume *= (1.0f - hollowVolume);
2079 } 2127 }
2080 } 2128 }
2081 break; 2129 break;
2082 2130
2083 case ProfileShape.HalfCircle: 2131 case ProfileShape.HalfCircle:
2084 if (_pbs.PathCurve == (byte)Extrusion.Curve1) 2132 if (_pbs.PathCurve == (byte)Extrusion.Curve1)
2085 { 2133 {
2086 volume *= 0.52359877559829887307710723054658f; 2134 volume *= 0.52359877559829887307710723054658f;
2087 } 2135 }
2088 break; 2136 break;
2089 2137
2090 case ProfileShape.EquilateralTriangle: 2138 case ProfileShape.EquilateralTriangle:
2091 2139
2092 if (_pbs.PathCurve == (byte)Extrusion.Straight) 2140 if (_pbs.PathCurve == (byte)Extrusion.Straight)
2093 { 2141 {
2094 volume *= 0.32475953f; 2142 volume *= 0.32475953f;
2095 2143
2096 if (hollowAmount > 0.0) 2144 if (hollowAmount > 0.0)
2097 { 2145 {
2098 2146
2099 // calculate the hollow volume by it's shape compared to the prim shape 2147 // calculate the hollow volume by it's shape compared to the prim shape
2100 switch (_pbs.HollowShape) 2148 switch (_pbs.HollowShape)
2101 { 2149 {
2102 case HollowShape.Same: 2150 case HollowShape.Same:
2103 case HollowShape.Triangle: 2151 case HollowShape.Triangle:
2104 hollowVolume *= .25f; 2152 hollowVolume *= .25f;
2105 break; 2153 break;
2106 2154
2107 case HollowShape.Square: 2155 case HollowShape.Square:
2108 hollowVolume *= 0.499849f * 3.07920140172638f; 2156 hollowVolume *= 0.499849f * 3.07920140172638f;
2109 break; 2157 break;
2110 2158
2111 case HollowShape.Circle: 2159 case HollowShape.Circle:
2112 // Hollow shape is a perfect cyllinder in respect to the cube's scale 2160 // Hollow shape is a perfect cyllinder in respect to the cube's scale
2113 // Cyllinder hollow volume calculation 2161 // Cyllinder hollow volume calculation
2114 2162
2115 hollowVolume *= 0.1963495f * 3.07920140172638f; 2163 hollowVolume *= 0.1963495f * 3.07920140172638f;
2116 break; 2164 break;
2117 2165
2118 default: 2166 default:
2119 hollowVolume = 0; 2167 hollowVolume = 0;
2120 break; 2168 break;
2121 } 2169 }
2122 volume *= (1.0f - hollowVolume); 2170 volume *= (1.0f - hollowVolume);
2123 } 2171 }
2124 } 2172 }
2125 else if (_pbs.PathCurve == (byte)Extrusion.Curve1) 2173 else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
2126 { 2174 {
2127 volume *= 0.32475953f; 2175 volume *= 0.32475953f;
2128 volume *= 0.01f * (float)(200 - _pbs.PathScaleX); 2176 volume *= 0.01f * (float)(200 - _pbs.PathScaleX);
2129 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); 2177 tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
2130 volume *= (1.0f - tmp * tmp); 2178 volume *= (1.0f - tmp * tmp);
2131 2179
2132 if (hollowAmount > 0.0) 2180 if (hollowAmount > 0.0)
2133 { 2181 {
2134 2182
2135 hollowVolume *= hollowAmount; 2183 hollowVolume *= hollowAmount;
2136 2184
2137 switch (_pbs.HollowShape) 2185 switch (_pbs.HollowShape)
2138 { 2186 {
2139 case HollowShape.Same: 2187 case HollowShape.Same:
2140 case HollowShape.Triangle: 2188 case HollowShape.Triangle:
2141 hollowVolume *= .25f; 2189 hollowVolume *= .25f;
2142 break; 2190 break;
2143 2191
2144 case HollowShape.Square: 2192 case HollowShape.Square:
2145 hollowVolume *= 0.499849f * 3.07920140172638f; 2193 hollowVolume *= 0.499849f * 3.07920140172638f;
2146 break; 2194 break;
2147 2195
2148 case HollowShape.Circle: 2196 case HollowShape.Circle:
2149 2197
2150 hollowVolume *= 0.1963495f * 3.07920140172638f; 2198 hollowVolume *= 0.1963495f * 3.07920140172638f;
2151 break; 2199 break;
2152 2200
2153 default: 2201 default:
2154 hollowVolume = 0; 2202 hollowVolume = 0;
2155 break; 2203 break;
2156 } 2204 }
2157 volume *= (1.0f - hollowVolume); 2205 volume *= (1.0f - hollowVolume);
2158 } 2206 }
2159 } 2207 }
2160 break; 2208 break;
2161 2209
2162 default: 2210 default:
2163 break; 2211 break;
2164 } 2212 }
2165 2213
2166 float taperX1; 2214 float taperX1;
2167 float taperY1; 2215 float taperY1;
2168 float taperX; 2216 float taperX;
2169 float taperY; 2217 float taperY;
2170 float pathBegin; 2218 float pathBegin;
2171 float pathEnd; 2219 float pathEnd;
2172 float profileBegin; 2220 float profileBegin;
2173 float profileEnd; 2221 float profileEnd;
2174 2222
2175 if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) 2223 if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible)
2176 { 2224 {
2177 taperX1 = _pbs.PathScaleX * 0.01f; 2225 taperX1 = _pbs.PathScaleX * 0.01f;
2178 if (taperX1 > 1.0f) 2226 if (taperX1 > 1.0f)
2179 taperX1 = 2.0f - taperX1; 2227 taperX1 = 2.0f - taperX1;
2180 taperX = 1.0f - taperX1; 2228 taperX = 1.0f - taperX1;
2181 2229
2182 taperY1 = _pbs.PathScaleY * 0.01f; 2230 taperY1 = _pbs.PathScaleY * 0.01f;
2183 if (taperY1 > 1.0f) 2231 if (taperY1 > 1.0f)
2184 taperY1 = 2.0f - taperY1; 2232 taperY1 = 2.0f - taperY1;
2185 taperY = 1.0f - taperY1; 2233 taperY = 1.0f - taperY1;
2186 } 2234 }
2187 else 2235 else
2188 { 2236 {
2189 taperX = _pbs.PathTaperX * 0.01f; 2237 taperX = _pbs.PathTaperX * 0.01f;
2190 if (taperX < 0.0f) 2238 if (taperX < 0.0f)
2191 taperX = -taperX; 2239 taperX = -taperX;
2192 taperX1 = 1.0f - taperX; 2240 taperX1 = 1.0f - taperX;
2193 2241
2194 taperY = _pbs.PathTaperY * 0.01f; 2242 taperY = _pbs.PathTaperY * 0.01f;
2195 if (taperY < 0.0f) 2243 if (taperY < 0.0f)
2196 taperY = -taperY; 2244 taperY = -taperY;
2197 taperY1 = 1.0f - taperY; 2245 taperY1 = 1.0f - taperY;
2198 } 2246 }
2199 2247
2200 volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); 2248 volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY);
2201 2249
2202 pathBegin = (float)_pbs.PathBegin * 2.0e-5f; 2250 pathBegin = (float)_pbs.PathBegin * 2.0e-5f;
2203 pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; 2251 pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f;
2204 volume *= (pathEnd - pathBegin); 2252 volume *= (pathEnd - pathBegin);
2205 2253
2206 // this is crude aproximation 2254 // this is crude aproximation
2207 profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; 2255 profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f;
2208 profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; 2256 profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f;
2209 volume *= (profileEnd - profileBegin); 2257 volume *= (profileEnd - profileBegin);
2210 2258
2211 return volume; 2259 return volume;
2212 } 2260 }
2213 2261
2214 2262
2215 private void CalcPrimBodyData() 2263 private void CalcPrimBodyData()
2216 { 2264 {
2217 float volume; 2265 float volume;
2218 2266
2219 if (prim_geom == IntPtr.Zero) 2267 if (prim_geom == IntPtr.Zero)
2220 { 2268 {
2221 // Ubit let's have a initial basic OOB 2269 // Ubit let's have a initial basic OOB
2222 primOOBsize.X = _size.X; 2270 primOOBsize.X = _size.X;
2223 primOOBsize.Y = _size.Y; 2271 primOOBsize.Y = _size.Y;
2224 primOOBsize.Z = _size.Z; 2272 primOOBsize.Z = _size.Z;
2225 primOOBoffset = Vector3.Zero; 2273 primOOBoffset = Vector3.Zero;
2226 } 2274 }
2227 else 2275 else
2228 { 2276 {
2229 d.AABB AABB; 2277 d.AABB AABB;
2230 d.GeomGetAABB(prim_geom, out AABB); // get the AABB from engine geom 2278 d.GeomGetAABB(prim_geom, out AABB); // get the AABB from engine geom
2231 2279
2232 primOOBsize.X = (AABB.MaxX - AABB.MinX); 2280 primOOBsize.X = (AABB.MaxX - AABB.MinX);
2233 primOOBsize.Y = (AABB.MaxY - AABB.MinY); 2281 primOOBsize.Y = (AABB.MaxY - AABB.MinY);
2234 primOOBsize.Z = (AABB.MaxZ - AABB.MinZ); 2282 primOOBsize.Z = (AABB.MaxZ - AABB.MinZ);
2235 if (!hasOOBoffsetFromMesh) 2283 if (!hasOOBoffsetFromMesh)
2236 { 2284 {
2237 primOOBoffset.X = (AABB.MaxX + AABB.MinX) * 0.5f; 2285 primOOBoffset.X = (AABB.MaxX + AABB.MinX) * 0.5f;
2238 primOOBoffset.Y = (AABB.MaxY + AABB.MinY) * 0.5f; 2286 primOOBoffset.Y = (AABB.MaxY + AABB.MinY) * 0.5f;
2239 primOOBoffset.Z = (AABB.MaxZ + AABB.MinZ) * 0.5f; 2287 primOOBoffset.Z = (AABB.MaxZ + AABB.MinZ) * 0.5f;
2240 } 2288 }
2241 } 2289 }
2242 2290
2243 // also its own inertia and mass 2291 // also its own inertia and mass
2244 // keep using basic shape mass for now 2292 // keep using basic shape mass for now
2245 volume = CalculatePrimVolume(); 2293 volume = CalculatePrimVolume();
2246 2294
2247 primMass = m_density * volume; 2295 primMass = m_density * volume;
2248 2296
2249 if (primMass <= 0) 2297 if (primMass <= 0)
2250 primMass = 0.0001f;//ckrinke: Mass must be greater then zero. 2298 primMass = 0.0001f;//ckrinke: Mass must be greater then zero.
2251 if (primMass > _parent_scene.maximumMassObject) 2299 if (primMass > _parent_scene.maximumMassObject)
2252 primMass = _parent_scene.maximumMassObject; 2300 primMass = _parent_scene.maximumMassObject;
2253 2301
2254 _mass = primMass; // just in case 2302 _mass = primMass; // just in case
2255 2303
2256 d.MassSetBoxTotal(out primdMass, primMass, primOOBsize.X, primOOBsize.Y, primOOBsize.Z); 2304 d.MassSetBoxTotal(out primdMass, primMass, primOOBsize.X, primOOBsize.Y, primOOBsize.Z);
2257 2305
2258 d.MassTranslate(ref primdMass, 2306 d.MassTranslate(ref primdMass,
2259 primOOBoffset.X, 2307 primOOBoffset.X,
2260 primOOBoffset.Y, 2308 primOOBoffset.Y,
2261 primOOBoffset.Z); 2309 primOOBoffset.Z);
2262 2310
2263 primOOBsize *= 0.5f; // let obb size be a corner coords 2311 primOOBsize *= 0.5f; // let obb size be a corner coords
2264 primOOBradiusSQ = primOOBsize.LengthSquared(); 2312 primOOBradiusSQ = primOOBsize.LengthSquared();
2265 } 2313 }
2266 2314
2267 2315
2268 #endregion 2316 #endregion
2269 2317
2270 2318
2271 /// <summary> 2319 /// <summary>
2272 /// Add a child prim to this parent prim. 2320 /// Add a child prim to this parent prim.
2273 /// </summary> 2321 /// </summary>
2274 /// <param name="prim">Child prim</param> 2322 /// <param name="prim">Child prim</param>
2275 // I'm the parent 2323 // I'm the parent
2276 // prim is the child 2324 // prim is the child
2277 public void ParentPrim(OdePrim prim) 2325 public void ParentPrim(OdePrim prim)
2278 { 2326 {
2279 //Console.WriteLine("ParentPrim " + m_primName); 2327 //Console.WriteLine("ParentPrim " + m_primName);
2280 if (this.m_localID != prim.m_localID) 2328 if (this.m_localID != prim.m_localID)
2281 { 2329 {
2282 DestroyBody(); // for now we need to rebuil entire object on link change 2330 DestroyBody(); // for now we need to rebuil entire object on link change
2283 2331
2284 lock (childrenPrim) 2332 lock (childrenPrim)
2285 { 2333 {
2286 // adopt the prim 2334 // adopt the prim
2287 if (!childrenPrim.Contains(prim)) 2335 if (!childrenPrim.Contains(prim))
2288 childrenPrim.Add(prim); 2336 childrenPrim.Add(prim);
2289 2337
2290 // see if this prim has kids and adopt them also 2338 // see if this prim has kids and adopt them also
2291 // should not happen for now 2339 // should not happen for now
2292 foreach (OdePrim prm in prim.childrenPrim) 2340 foreach (OdePrim prm in prim.childrenPrim)
2293 { 2341 {
2294 if (!childrenPrim.Contains(prm)) 2342 if (!childrenPrim.Contains(prm))
2295 { 2343 {
2296 if (prm.Body != IntPtr.Zero) 2344 if (prm.Body != IntPtr.Zero)
2297 { 2345 {
2298 if (prm.prim_geom != IntPtr.Zero) 2346 if (prm.prim_geom != IntPtr.Zero)
2299 d.GeomSetBody(prm.prim_geom, IntPtr.Zero); 2347 d.GeomSetBody(prm.prim_geom, IntPtr.Zero);
2300 if (prm.Body != prim.Body) 2348 if (prm.Body != prim.Body)
2301 prm.DestroyBody(); // don't loose bodies around 2349 prm.DestroyBody(); // don't loose bodies around
2302 prm.Body = IntPtr.Zero; 2350 prm.Body = IntPtr.Zero;
2303 } 2351 }
2304 2352
2305 childrenPrim.Add(prm); 2353 childrenPrim.Add(prm);
2306 prm._parent = this; 2354 prm._parent = this;
2307 } 2355 }
2308 } 2356 }
2309 } 2357 }
2310 //Remove old children from the prim 2358 //Remove old children from the prim
2311 prim.childrenPrim.Clear(); 2359 prim.childrenPrim.Clear();
2312 2360
2313 if (prim.Body != IntPtr.Zero) 2361 if (prim.Body != IntPtr.Zero)
2314 { 2362 {
2315 if (prim.prim_geom != IntPtr.Zero) 2363 if (prim.prim_geom != IntPtr.Zero)
2316 d.GeomSetBody(prim.prim_geom, IntPtr.Zero); 2364 d.GeomSetBody(prim.prim_geom, IntPtr.Zero);
2317 prim.DestroyBody(); // don't loose bodies around 2365 prim.DestroyBody(); // don't loose bodies around
2318 prim.Body = IntPtr.Zero; 2366 prim.Body = IntPtr.Zero;
2319 } 2367 }
2320 2368
2321 prim.childPrim = true; 2369 prim.childPrim = true;
2322 prim._parent = this; 2370 prim._parent = this;
2323 2371
2324 MakeBody(); // full nasty reconstruction 2372 MakeBody(); // full nasty reconstruction
2325 } 2373 }
2326 } 2374 }
2327 2375
2328 private void UpdateChildsfromgeom() 2376 private void UpdateChildsfromgeom()
2329 { 2377 {
2330 if (childrenPrim.Count > 0) 2378 if (childrenPrim.Count > 0)
2331 { 2379 {
2332 foreach (OdePrim prm in childrenPrim) 2380 foreach (OdePrim prm in childrenPrim)
2333 prm.UpdateDataFromGeom(); 2381 prm.UpdateDataFromGeom();
2334 } 2382 }
2335 } 2383 }
2336 2384
2337 private void UpdateDataFromGeom() 2385 private void UpdateDataFromGeom()
2338 { 2386 {
2339 if (prim_geom != IntPtr.Zero) 2387 if (prim_geom != IntPtr.Zero)
2340 { 2388 {
2341 d.Quaternion qtmp = new d.Quaternion { }; 2389 d.Quaternion qtmp = new d.Quaternion { };
2342 d.GeomCopyQuaternion(prim_geom, out qtmp); 2390 d.GeomCopyQuaternion(prim_geom, out qtmp);
2343 _orientation.W = qtmp.W; 2391 _orientation.W = qtmp.W;
2344 _orientation.X = qtmp.X; 2392 _orientation.X = qtmp.X;
2345 _orientation.Y = qtmp.Y; 2393 _orientation.Y = qtmp.Y;
2346 _orientation.Z = qtmp.Z; 2394 _orientation.Z = qtmp.Z;
2347 2395
2348 d.Vector3 lpos; 2396 d.Vector3 lpos;
2349 d.GeomCopyPosition(prim_geom, out lpos); 2397 d.GeomCopyPosition(prim_geom, out lpos);
2350 _position.X = lpos.X; 2398 _position.X = lpos.X;
2351 _position.Y = lpos.Y; 2399 _position.Y = lpos.Y;
2352 _position.Z = lpos.Z; 2400 _position.Z = lpos.Z;
2353 } 2401 }
2354 } 2402 }
2355 2403
2356 private void ChildDelink(OdePrim odePrim, bool remakebodies) 2404 private void ChildDelink(OdePrim odePrim, bool remakebodies)
2357 { 2405 {
2358 // Okay, we have a delinked child.. destroy all body and remake 2406 // Okay, we have a delinked child.. destroy all body and remake
2359 if (odePrim != this && !childrenPrim.Contains(odePrim)) 2407 if (odePrim != this && !childrenPrim.Contains(odePrim))
2360 return; 2408 return;
2361 2409
2362 DestroyBody(); 2410 DestroyBody();
2363 2411
2364 if (odePrim == this) // delinking the root prim 2412 if (odePrim == this) // delinking the root prim
2365 { 2413 {
2366 OdePrim newroot = null; 2414 OdePrim newroot = null;
2367 lock (childrenPrim) 2415 lock (childrenPrim)
2368 { 2416 {
2369 if (childrenPrim.Count > 0) 2417 if (childrenPrim.Count > 0)
2370 { 2418 {
2371 newroot = childrenPrim[0]; 2419 newroot = childrenPrim[0];
2372 childrenPrim.RemoveAt(0); 2420 childrenPrim.RemoveAt(0);
2373 foreach (OdePrim prm in childrenPrim) 2421 foreach (OdePrim prm in childrenPrim)
2374 { 2422 {
2375 newroot.childrenPrim.Add(prm); 2423 newroot.childrenPrim.Add(prm);
2376 } 2424 }
2377 childrenPrim.Clear(); 2425 childrenPrim.Clear();
2378 } 2426 }
2379 if (newroot != null) 2427 if (newroot != null)
2380 { 2428 {
2381 newroot.childPrim = false; 2429 newroot.childPrim = false;
2382 newroot._parent = null; 2430 newroot._parent = null;
2383 if (remakebodies) 2431 if (remakebodies)
2384 newroot.MakeBody(); 2432 newroot.MakeBody();
2385 } 2433 }
2386 } 2434 }
2387 } 2435 }
2388 2436
2389 else 2437 else
2390 { 2438 {
2391 lock (childrenPrim) 2439 lock (childrenPrim)
2392 { 2440 {
2393 childrenPrim.Remove(odePrim); 2441 childrenPrim.Remove(odePrim);
2394 odePrim.childPrim = false; 2442 odePrim.childPrim = false;
2395 odePrim._parent = null; 2443 odePrim._parent = null;
2396 // odePrim.UpdateDataFromGeom(); 2444 // odePrim.UpdateDataFromGeom();
2397 if (remakebodies) 2445 if (remakebodies)
2398 odePrim.MakeBody(); 2446 odePrim.MakeBody();
2399 } 2447 }
2400 } 2448 }
2401 if (remakebodies) 2449 if (remakebodies)
2402 MakeBody(); 2450 MakeBody();
2403 } 2451 }
2404 2452
2405 protected void ChildRemove(OdePrim odePrim, bool reMakeBody) 2453 protected void ChildRemove(OdePrim odePrim, bool reMakeBody)
2406 { 2454 {
2407 // Okay, we have a delinked child.. destroy all body and remake 2455 // Okay, we have a delinked child.. destroy all body and remake
2408 if (odePrim != this && !childrenPrim.Contains(odePrim)) 2456 if (odePrim != this && !childrenPrim.Contains(odePrim))
2409 return; 2457 return;
2410 2458
2411 DestroyBody(); 2459 DestroyBody();
2412 2460
2413 if (odePrim == this) 2461 if (odePrim == this)
2414 { 2462 {
2415 OdePrim newroot = null; 2463 OdePrim newroot = null;
2416 lock (childrenPrim) 2464 lock (childrenPrim)
2417 { 2465 {
2418 if (childrenPrim.Count > 0) 2466 if (childrenPrim.Count > 0)
2419 { 2467 {
2420 newroot = childrenPrim[0]; 2468 newroot = childrenPrim[0];
2421 childrenPrim.RemoveAt(0); 2469 childrenPrim.RemoveAt(0);
2422 foreach (OdePrim prm in childrenPrim) 2470 foreach (OdePrim prm in childrenPrim)
2423 { 2471 {
2424 newroot.childrenPrim.Add(prm); 2472 newroot.childrenPrim.Add(prm);
2425 } 2473 }
2426 childrenPrim.Clear(); 2474 childrenPrim.Clear();
2427 } 2475 }
2428 if (newroot != null) 2476 if (newroot != null)
2429 { 2477 {
2430 newroot.childPrim = false; 2478 newroot.childPrim = false;
2431 newroot._parent = null; 2479 newroot._parent = null;
2432 newroot.MakeBody(); 2480 newroot.MakeBody();
2433 } 2481 }
2434 } 2482 }
2435 if (reMakeBody) 2483 if (reMakeBody)
2436 MakeBody(); 2484 MakeBody();
2437 return; 2485 return;
2438 } 2486 }
2439 else 2487 else
2440 { 2488 {
2441 lock (childrenPrim) 2489 lock (childrenPrim)
2442 { 2490 {
2443 childrenPrim.Remove(odePrim); 2491 childrenPrim.Remove(odePrim);
2444 odePrim.childPrim = false; 2492 odePrim.childPrim = false;
2445 odePrim._parent = null; 2493 odePrim._parent = null;
2446 if (reMakeBody) 2494 if (reMakeBody)
2447 odePrim.MakeBody(); 2495 odePrim.MakeBody();
2448 } 2496 }
2449 } 2497 }
2450 MakeBody(); 2498 MakeBody();
2451 } 2499 }
2452 2500
2453 #region changes 2501 #region changes
2454 2502
2455 private void changeadd() 2503 private void changeadd()
2456 { 2504 {
2457 CreateGeom(); 2505 CreateGeom();
2458 2506
2459 if (prim_geom != IntPtr.Zero) 2507 if (prim_geom != IntPtr.Zero)
2460 { 2508 {
2461 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); 2509 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
2462 d.Quaternion myrot = new d.Quaternion(); 2510 d.Quaternion myrot = new d.Quaternion();
2463 myrot.X = _orientation.X; 2511 myrot.X = _orientation.X;
2464 myrot.Y = _orientation.Y; 2512 myrot.Y = _orientation.Y;
2465 myrot.Z = _orientation.Z; 2513 myrot.Z = _orientation.Z;
2466 myrot.W = _orientation.W; 2514 myrot.W = _orientation.W;
2467 d.GeomSetQuaternion(prim_geom, ref myrot); 2515 d.GeomSetQuaternion(prim_geom, ref myrot);
2468 2516
2469 if (!m_isphysical) 2517 if (!m_isphysical)
2470 SetInStaticSpace(this); 2518 SetInStaticSpace(this);
2471 } 2519 }
2472 2520
2473 if (m_isphysical && Body == IntPtr.Zero) 2521 if (m_isphysical && Body == IntPtr.Zero)
2474 { 2522 {
2475 MakeBody(); 2523 MakeBody();
2476 } 2524 }
2477 } 2525 }
2478 2526
2479 private void changeAngularLock(Vector3 newLock) 2527 private void changeAngularLock(Vector3 newLock)
2480 { 2528 {
2481 // do we have a Physical object? 2529 // do we have a Physical object?
2482 if (Body != IntPtr.Zero) 2530 if (Body != IntPtr.Zero)
2483 { 2531 {
2484 //Check that we have a Parent 2532 //Check that we have a Parent
2485 //If we have a parent then we're not authorative here 2533 //If we have a parent then we're not authorative here
2486 if (_parent == null) 2534 if (_parent == null)
2487 { 2535 {
2488 if (!newLock.ApproxEquals(Vector3.One, 0f)) 2536 if (!newLock.ApproxEquals(Vector3.One, 0f))
2489 { 2537 {
2490 createAMotor(newLock); 2538 createAMotor(newLock);
2491 } 2539 }
2492 else 2540 else
2493 { 2541 {
2494 if (Amotor != IntPtr.Zero) 2542 if (Amotor != IntPtr.Zero)
2495 { 2543 {
2496 d.JointDestroy(Amotor); 2544 d.JointDestroy(Amotor);
2497 Amotor = IntPtr.Zero; 2545 Amotor = IntPtr.Zero;
2498 } 2546 }
2499 } 2547 }
2500 } 2548 }
2501 } 2549 }
2502 // Store this for later in case we get turned into a separate body 2550 // Store this for later in case we get turned into a separate body
2503 m_angularlock = newLock; 2551 m_angularlock = newLock;
2504 } 2552 }
2505 2553
2506 private void changeLink(OdePrim NewParent) 2554 private void changeLink(OdePrim NewParent)
2507 { 2555 {
2508 if (_parent == null && NewParent != null) 2556 if (_parent == null && NewParent != null)
2509 { 2557 {
2510 NewParent.ParentPrim(this); 2558 NewParent.ParentPrim(this);
2511 } 2559 }
2512 else if (_parent != null) 2560 else if (_parent != null)
2513 { 2561 {
2514 if (_parent is OdePrim) 2562 if (_parent is OdePrim)
2515 { 2563 {
2516 if (NewParent != _parent) 2564 if (NewParent != _parent)
2517 { 2565 {
2518 (_parent as OdePrim).ChildDelink(this, false); // for now... 2566 (_parent as OdePrim).ChildDelink(this, false); // for now...
2519 childPrim = false; 2567 childPrim = false;
2520 2568
2521 if (NewParent != null) 2569 if (NewParent != null)
2522 { 2570 {
2523 NewParent.ParentPrim(this); 2571 NewParent.ParentPrim(this);
2524 } 2572 }
2525 } 2573 }
2526 } 2574 }
2527 } 2575 }
2528 _parent = NewParent; 2576 _parent = NewParent;
2529 } 2577 }
2530 2578
2531 2579
2532 private void Stop() 2580 private void Stop()
2533 { 2581 {
2534 if (!childPrim) 2582 if (!childPrim)
2535 { 2583 {
2536 m_force = Vector3.Zero; 2584 m_force = Vector3.Zero;
2537 m_forceacc = Vector3.Zero; 2585 m_forceacc = Vector3.Zero;
2538 m_angularForceacc = Vector3.Zero; 2586 m_angularForceacc = Vector3.Zero;
2539 _torque = Vector3.Zero; 2587 _torque = Vector3.Zero;
2540 _velocity = Vector3.Zero; 2588 _velocity = Vector3.Zero;
2541 _acceleration = Vector3.Zero; 2589 _acceleration = Vector3.Zero;
2542 m_rotationalVelocity = Vector3.Zero; 2590 m_rotationalVelocity = Vector3.Zero;
2543 _target_velocity = Vector3.Zero; 2591 _target_velocity = Vector3.Zero;
2544 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) 2592 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
2545 m_vehicle.Stop(); 2593 m_vehicle.Stop();
2546 } 2594 }
2547 2595
2548 if (Body != IntPtr.Zero) 2596 if (Body != IntPtr.Zero)
2549 { 2597 {
2550 d.BodySetForce(Body, 0f, 0f, 0f); 2598 d.BodySetForce(Body, 0f, 0f, 0f);
2551 d.BodySetTorque(Body, 0f, 0f, 0f); 2599 d.BodySetTorque(Body, 0f, 0f, 0f);
2552 d.BodySetLinearVel(Body, 0f, 0f, 0f); 2600 d.BodySetLinearVel(Body, 0f, 0f, 0f);
2553 d.BodySetAngularVel(Body, 0f, 0f, 0f); 2601 d.BodySetAngularVel(Body, 0f, 0f, 0f);
2554 } 2602 }
2555 } 2603 }
2556 2604
2557 2605
2558 private void changePhantomStatus(bool newval) 2606 private void changePhantomStatus(bool newval)
2559 { 2607 {
2560 m_isphantom = newval; 2608 m_isphantom = newval;
2561 2609
2562 if (m_isSelected) 2610 if (m_isSelected)
2563 { 2611 {
2564 m_collisionCategories = CollisionCategories.Selected; 2612 m_collisionCategories = CollisionCategories.Selected;
2565 m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); 2613 m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
2566 } 2614 }
2567 else 2615 else
2568 { 2616 {
2569 if (m_isphantom && !m_isVolumeDetect) 2617 if (m_isphantom && !m_isVolumeDetect)
2570 { 2618 {
2571 m_collisionCategories = 0; 2619 m_collisionCategories = 0;
2572 if (m_isphysical) 2620 if (m_isphysical)
2573 m_collisionFlags = CollisionCategories.Land; 2621 m_collisionFlags = CollisionCategories.Land;
2574 else 2622 else
2575 m_collisionFlags = 0; // should never happen 2623 m_collisionFlags = 0; // should never happen
2576 } 2624 }
2577 2625
2578 else 2626 else
2579 { 2627 {
2580 m_collisionCategories = CollisionCategories.Geom; 2628 m_collisionCategories = CollisionCategories.Geom;
2581 if (m_isphysical) 2629 if (m_isphysical)
2582 m_collisionCategories |= CollisionCategories.Body; 2630 m_collisionCategories |= CollisionCategories.Body;
2583 2631
2584 m_collisionFlags = m_default_collisionFlags | CollisionCategories.Land; 2632 m_collisionFlags = m_default_collisionFlags | CollisionCategories.Land;
2585 2633
2586 if (m_collidesWater) 2634 if (m_collidesWater)
2587 m_collisionFlags |= CollisionCategories.Water; 2635 m_collisionFlags |= CollisionCategories.Water;
2588 } 2636 }
2589 } 2637 }
2590 2638
2591 if (!childPrim) 2639 if (!childPrim)
2592 { 2640 {
2593 foreach (OdePrim prm in childrenPrim) 2641 foreach (OdePrim prm in childrenPrim)
2594 { 2642 {
2595 prm.m_collisionCategories = m_collisionCategories; 2643 prm.m_collisionCategories = m_collisionCategories;
2596 prm.m_collisionFlags = m_collisionFlags; 2644 prm.m_collisionFlags = m_collisionFlags;
2597 2645
2598 if (!prm.m_disabled && prm.prim_geom != IntPtr.Zero) 2646 if (!prm.m_disabled && prm.prim_geom != IntPtr.Zero)
2599 { 2647 {
2600 if (prm.m_NoColide) 2648 if (prm.m_NoColide)
2601 { 2649 {
2602 d.GeomSetCategoryBits(prm.prim_geom, 0); 2650 d.GeomSetCategoryBits(prm.prim_geom, 0);
2603 if (m_isphysical) 2651 if (m_isphysical)
2604 d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); 2652 d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land);
2605 else 2653 else
2606 d.GeomSetCollideBits(prm.prim_geom, 0); 2654 d.GeomSetCollideBits(prm.prim_geom, 0);
2607 } 2655 }
2608 else 2656 else
2609 { 2657 {
2610 d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); 2658 d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories);
2611 d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); 2659 d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags);
2612 } 2660 }
2613 if(!m_isSelected) 2661 if(!m_isSelected)
2614 d.GeomEnable(prm.prim_geom); 2662 d.GeomEnable(prm.prim_geom);
2615 } 2663 }
2616 } 2664 }
2617 } 2665 }
2618 2666
2619 if (!m_disabled && prim_geom != IntPtr.Zero) 2667 if (!m_disabled && prim_geom != IntPtr.Zero)
2620 { 2668 {
2621 if (m_NoColide) 2669 if (m_NoColide)
2622 { 2670 {
2623 d.GeomSetCategoryBits(prim_geom, 0); 2671 d.GeomSetCategoryBits(prim_geom, 0);
2624 if (m_isphysical) 2672 if (m_isphysical)
2625 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); 2673 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
2626 else 2674 else
2627 d.GeomSetCollideBits(prim_geom, 0); 2675 d.GeomSetCollideBits(prim_geom, 0);
2628 } 2676 }
2629 else 2677 else
2630 { 2678 {
2631 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 2679 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
2632 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 2680 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
2633 } 2681 }
2634 if(!m_isSelected) 2682 if(!m_isSelected)
2635 d.GeomEnable(prim_geom); 2683 d.GeomEnable(prim_geom);
2636 } 2684 }
2637 } 2685 }
2638 2686
2639 private void changeSelectedStatus(bool newval) 2687 private void changeSelectedStatus(bool newval)
2640 { 2688 {
2641 if (m_lastdoneSelected == newval) 2689 if (m_lastdoneSelected == newval)
2642 return; 2690 return;
2643 2691
2644 m_lastdoneSelected = newval; 2692 m_lastdoneSelected = newval;
2645 DoSelectedStatus(newval); 2693 DoSelectedStatus(newval);
2646 } 2694 }
2647 2695
2648 private void CheckDelaySelect() 2696 private void CheckDelaySelect()
2649 { 2697 {
2650 if (m_delaySelect) 2698 if (m_delaySelect)
2651 { 2699 {
2652 DoSelectedStatus(m_isSelected); 2700 DoSelectedStatus(m_isSelected);
2653 } 2701 }
2654 } 2702 }
2655 2703
2656 private void DoSelectedStatus(bool newval) 2704 private void DoSelectedStatus(bool newval)
2657 { 2705 {
2658 m_isSelected = newval; 2706 m_isSelected = newval;
2659 Stop(); 2707 Stop();
2660 2708
2661 if (newval) 2709 if (newval)
2662 { 2710 {
2663 if (!childPrim && Body != IntPtr.Zero) 2711 if (!childPrim && Body != IntPtr.Zero)
2664 d.BodyDisable(Body); 2712 d.BodyDisable(Body);
2665 2713
2666 if (m_delaySelect || m_isphysical) 2714 if (m_delaySelect || m_isphysical)
2667 { 2715 {
2668 m_collisionCategories = CollisionCategories.Selected; 2716 m_collisionCategories = CollisionCategories.Selected;
2669 m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space); 2717 m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
2670 2718
2671 if (!childPrim) 2719 if (!childPrim)
2672 { 2720 {
2673 foreach (OdePrim prm in childrenPrim) 2721 foreach (OdePrim prm in childrenPrim)
2674 { 2722 {
2675 prm.m_collisionCategories = m_collisionCategories; 2723 prm.m_collisionCategories = m_collisionCategories;
2676 prm.m_collisionFlags = m_collisionFlags; 2724 prm.m_collisionFlags = m_collisionFlags;
2677 2725
2678 if (prm.prim_geom != null) 2726 if (prm.prim_geom != null)
2679 { 2727 {
2680 2728
2681 if (prm.m_NoColide) 2729 if (prm.m_NoColide)
2682 { 2730 {
2683 d.GeomSetCategoryBits(prm.prim_geom, 0); 2731 d.GeomSetCategoryBits(prm.prim_geom, 0);
2684 d.GeomSetCollideBits(prm.prim_geom, 0); 2732 d.GeomSetCollideBits(prm.prim_geom, 0);
2685 } 2733 }
2686 else 2734 else
2687 { 2735 {
2688 d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); 2736 d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories);
2689 d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); 2737 d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags);
2690 } 2738 }
2691 d.GeomDisable(prm.prim_geom); 2739 d.GeomDisable(prm.prim_geom);
2692 } 2740 }
2693 prm.m_delaySelect = false; 2741 prm.m_delaySelect = false;
2694 } 2742 }
2695 } 2743 }
2696 2744
2697 if (prim_geom != null) 2745 if (prim_geom != null)
2698 { 2746 {
2699 if (m_NoColide) 2747 if (m_NoColide)
2700 { 2748 {
2701 d.GeomSetCategoryBits(prim_geom, 0); 2749 d.GeomSetCategoryBits(prim_geom, 0);
2702 d.GeomSetCollideBits(prim_geom, 0); 2750 d.GeomSetCollideBits(prim_geom, 0);
2703 } 2751 }
2704 else 2752 else
2705 { 2753 {
2706 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 2754 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
2707 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 2755 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
2708 } 2756 }
2709 d.GeomDisable(prim_geom); 2757 d.GeomDisable(prim_geom);
2710 } 2758 }
2711 2759
2712 m_delaySelect = false; 2760 m_delaySelect = false;
2713 } 2761 }
2714 else if(!m_isphysical) 2762 else if(!m_isphysical)
2715 { 2763 {
2716 m_delaySelect = true; 2764 m_delaySelect = true;
2717 } 2765 }
2718 } 2766 }
2719 else 2767 else
2720 { 2768 {
2721 if (!childPrim && Body != IntPtr.Zero && !m_disabled) 2769 if (!childPrim && Body != IntPtr.Zero && !m_disabled)
2722 d.BodyEnable(Body); 2770 d.BodyEnable(Body);
2723 2771
2724 if (m_isphantom && !m_isVolumeDetect) 2772 if (m_isphantom && !m_isVolumeDetect)
2725 { 2773 {
2726 m_collisionCategories = 0; 2774 m_collisionCategories = 0;
2727 if(m_isphysical) 2775 if(m_isphysical)
2728 m_collisionFlags = CollisionCategories.Land; 2776 m_collisionFlags = CollisionCategories.Land;
2729 else 2777 else
2730 m_collisionFlags = 0; 2778 m_collisionFlags = 0;
2731 } 2779 }
2732 else 2780 else
2733 { 2781 {
2734 m_collisionCategories = CollisionCategories.Geom; 2782 m_collisionCategories = CollisionCategories.Geom;
2735 if (m_isphysical) 2783 if (m_isphysical)
2736 m_collisionCategories |= CollisionCategories.Body; 2784 m_collisionCategories |= CollisionCategories.Body;
2737 2785
2738 m_collisionFlags = m_default_collisionFlags | CollisionCategories.Land; 2786 m_collisionFlags = m_default_collisionFlags | CollisionCategories.Land;
2739 2787
2740 if (m_collidesWater) 2788 if (m_collidesWater)
2741 m_collisionFlags |= CollisionCategories.Water; 2789 m_collisionFlags |= CollisionCategories.Water;
2742 } 2790 }
2743 2791
2744 if (!childPrim) 2792 if (!childPrim)
2745 { 2793 {
2746 foreach (OdePrim prm in childrenPrim) 2794 foreach (OdePrim prm in childrenPrim)
2747 { 2795 {
2748 prm.m_collisionCategories = m_collisionCategories; 2796 prm.m_collisionCategories = m_collisionCategories;
2749 prm.m_collisionFlags = m_collisionFlags; 2797 prm.m_collisionFlags = m_collisionFlags;
2750 2798
2751 if (!prm.m_disabled && prm.prim_geom != IntPtr.Zero) 2799 if (!prm.m_disabled && prm.prim_geom != IntPtr.Zero)
2752 { 2800 {
2753 if (prm.m_NoColide) 2801 if (prm.m_NoColide)
2754 { 2802 {
2755 d.GeomSetCategoryBits(prm.prim_geom, 0); 2803 d.GeomSetCategoryBits(prm.prim_geom, 0);
2756 if (m_isphysical) 2804 if (m_isphysical)
2757 d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land); 2805 d.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land);
2758 else 2806 else
2759 d.GeomSetCollideBits(prm.prim_geom, 0); 2807 d.GeomSetCollideBits(prm.prim_geom, 0);
2760 } 2808 }
2761 else 2809 else
2762 { 2810 {
2763 d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories); 2811 d.GeomSetCategoryBits(prm.prim_geom, (int)m_collisionCategories);
2764 d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags); 2812 d.GeomSetCollideBits(prm.prim_geom, (int)m_collisionFlags);
2765 } 2813 }
2766 d.GeomEnable(prm.prim_geom); 2814 d.GeomEnable(prm.prim_geom);
2767 } 2815 }
2768 prm.m_delaySelect = false; 2816 prm.m_delaySelect = false;
2769 prm.m_softcolide = true; 2817 prm.m_softcolide = true;
2770 } 2818 }
2771 } 2819 }
2772 2820
2773 if (!m_disabled && prim_geom != IntPtr.Zero) 2821 if (!m_disabled && prim_geom != IntPtr.Zero)
2774 { 2822 {
2775 if (m_NoColide) 2823 if (m_NoColide)
2776 { 2824 {
2777 d.GeomSetCategoryBits(prim_geom, 0); 2825 d.GeomSetCategoryBits(prim_geom, 0);
2778 if (m_isphysical) 2826 if (m_isphysical)
2779 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land); 2827 d.GeomSetCollideBits(prim_geom, (int)CollisionCategories.Land);
2780 else 2828 else
2781 d.GeomSetCollideBits(prim_geom, 0); 2829 d.GeomSetCollideBits(prim_geom, 0);
2782 } 2830 }
2783 else 2831 else
2784 { 2832 {
2785 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories); 2833 d.GeomSetCategoryBits(prim_geom, (int)m_collisionCategories);
2786 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 2834 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
2787 } 2835 }
2788 d.GeomEnable(prim_geom); 2836 d.GeomEnable(prim_geom);
2789 } 2837 }
2790 2838
2791 m_delaySelect = false; 2839 m_delaySelect = false;
2792 m_softcolide = true; 2840 m_softcolide = true;
2793 } 2841 }
2794 2842
2795 resetCollisionAccounting(); 2843 resetCollisionAccounting();
2796 } 2844 }
2797 2845
2798 private void changePosition(Vector3 newPos) 2846 private void changePosition(Vector3 newPos)
2799 { 2847 {
2800 CheckDelaySelect(); 2848 CheckDelaySelect();
2801 if (m_isphysical) 2849 if (m_isphysical)
2802 { 2850 {
2803 if (childPrim) // inertia is messed, must rebuild 2851 if (childPrim) // inertia is messed, must rebuild
2804 { 2852 {
2805 if (m_building) 2853 if (m_building)
2806 { 2854 {
2807 _position = newPos; 2855 _position = newPos;
2808 } 2856 }
2809 2857
2810 else if (m_forcePosOrRotation && _position != newPos && Body != IntPtr.Zero) 2858 else if (m_forcePosOrRotation && _position != newPos && Body != IntPtr.Zero)
2811 { 2859 {
2812 FixInertia(newPos); 2860 FixInertia(newPos);
2813 if (!d.BodyIsEnabled(Body)) 2861 if (!d.BodyIsEnabled(Body))
2814 d.BodyEnable(Body); 2862 d.BodyEnable(Body);
2815 } 2863 }
2816 } 2864 }
2817 else 2865 else
2818 { 2866 {
2819 if (_position != newPos) 2867 if (_position != newPos)
2820 { 2868 {
2821 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); 2869 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2822 _position = newPos; 2870 _position = newPos;
2823 } 2871 }
2824 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) 2872 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2825 d.BodyEnable(Body); 2873 d.BodyEnable(Body);
2826 } 2874 }
2827 } 2875 }
2828 else 2876 else
2829 { 2877 {
2830 if (prim_geom != IntPtr.Zero) 2878 if (prim_geom != IntPtr.Zero)
2831 { 2879 {
2832 if (newPos != _position) 2880 if (newPos != _position)
2833 { 2881 {
2834 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); 2882 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2835 _position = newPos; 2883 _position = newPos;
2836 2884
2837 m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); 2885 m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace);
2838 } 2886 }
2839 } 2887 }
2840 } 2888 }
2841 givefakepos--; 2889 givefakepos--;
2842 if (givefakepos < 0) 2890 if (givefakepos < 0)
2843 givefakepos = 0; 2891 givefakepos = 0;
2844 // changeSelectedStatus(); 2892 // changeSelectedStatus();
2845 m_softcolide = true; 2893 m_softcolide = true;
2846 resetCollisionAccounting(); 2894 resetCollisionAccounting();
2847 } 2895 }
2848 2896
2849 private void changeOrientation(Quaternion newOri) 2897 private void changeOrientation(Quaternion newOri)
2850 { 2898 {
2851 CheckDelaySelect(); 2899 CheckDelaySelect();
2852 if (m_isphysical) 2900 if (m_isphysical)
2853 { 2901 {
2854 if (childPrim) // inertia is messed, must rebuild 2902 if (childPrim) // inertia is messed, must rebuild
2855 { 2903 {
2856 if (m_building) 2904 if (m_building)
2857 { 2905 {
2858 _orientation = newOri; 2906 _orientation = newOri;
2859 } 2907 }
2860 /* 2908 /*
2861 else if (m_forcePosOrRotation && _orientation != newOri && Body != IntPtr.Zero) 2909 else if (m_forcePosOrRotation && _orientation != newOri && Body != IntPtr.Zero)
2862 { 2910 {
2863 FixInertia(_position, newOri); 2911 FixInertia(_position, newOri);
2864 if (!d.BodyIsEnabled(Body)) 2912 if (!d.BodyIsEnabled(Body))
2865 d.BodyEnable(Body); 2913 d.BodyEnable(Body);
2866 } 2914 }
2867 */ 2915 */
2868 } 2916 }
2869 else 2917 else
2870 { 2918 {
2871 if (newOri != _orientation) 2919 if (newOri != _orientation)
2872 { 2920 {
2873 d.Quaternion myrot = new d.Quaternion(); 2921 d.Quaternion myrot = new d.Quaternion();
2874 myrot.X = newOri.X; 2922 myrot.X = newOri.X;
2875 myrot.Y = newOri.Y; 2923 myrot.Y = newOri.Y;
2876 myrot.Z = newOri.Z; 2924 myrot.Z = newOri.Z;
2877 myrot.W = newOri.W; 2925 myrot.W = newOri.W;
2878 d.GeomSetQuaternion(prim_geom, ref myrot); 2926 d.GeomSetQuaternion(prim_geom, ref myrot);
2879 _orientation = newOri; 2927 _orientation = newOri;
2880 if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) 2928 if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
2881 createAMotor(m_angularlock); 2929 createAMotor(m_angularlock);
2882 } 2930 }
2883 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) 2931 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2884 d.BodyEnable(Body); 2932 d.BodyEnable(Body);
2885 } 2933 }
2886 } 2934 }
2887 else 2935 else
2888 { 2936 {
2889 if (prim_geom != IntPtr.Zero) 2937 if (prim_geom != IntPtr.Zero)
2890 { 2938 {
2891 if (newOri != _orientation) 2939 if (newOri != _orientation)
2892 { 2940 {
2893 d.Quaternion myrot = new d.Quaternion(); 2941 d.Quaternion myrot = new d.Quaternion();
2894 myrot.X = newOri.X; 2942 myrot.X = newOri.X;
2895 myrot.Y = newOri.Y; 2943 myrot.Y = newOri.Y;
2896 myrot.Z = newOri.Z; 2944 myrot.Z = newOri.Z;
2897 myrot.W = newOri.W; 2945 myrot.W = newOri.W;
2898 d.GeomSetQuaternion(prim_geom, ref myrot); 2946 d.GeomSetQuaternion(prim_geom, ref myrot);
2899 _orientation = newOri; 2947 _orientation = newOri;
2900 } 2948 }
2901 } 2949 }
2902 } 2950 }
2903 givefakeori--; 2951 givefakeori--;
2904 if (givefakeori < 0) 2952 if (givefakeori < 0)
2905 givefakeori = 0; 2953 givefakeori = 0;
2906 m_softcolide = true; 2954 m_softcolide = true;
2907 resetCollisionAccounting(); 2955 resetCollisionAccounting();
2908 } 2956 }
2909 2957
2910 private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri) 2958 private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri)
2911 { 2959 {
2912 CheckDelaySelect(); 2960 CheckDelaySelect();
2913 if (m_isphysical) 2961 if (m_isphysical)
2914 { 2962 {
2915 if (childPrim && m_building) // inertia is messed, must rebuild 2963 if (childPrim && m_building) // inertia is messed, must rebuild
2916 { 2964 {
2917 _position = newPos; 2965 _position = newPos;
2918 _orientation = newOri; 2966 _orientation = newOri;
2919 } 2967 }
2920 else 2968 else
2921 { 2969 {
2922 if (newOri != _orientation) 2970 if (newOri != _orientation)
2923 { 2971 {
2924 d.Quaternion myrot = new d.Quaternion(); 2972 d.Quaternion myrot = new d.Quaternion();
2925 myrot.X = newOri.X; 2973 myrot.X = newOri.X;
2926 myrot.Y = newOri.Y; 2974 myrot.Y = newOri.Y;
2927 myrot.Z = newOri.Z; 2975 myrot.Z = newOri.Z;
2928 myrot.W = newOri.W; 2976 myrot.W = newOri.W;
2929 d.GeomSetQuaternion(prim_geom, ref myrot); 2977 d.GeomSetQuaternion(prim_geom, ref myrot);
2930 _orientation = newOri; 2978 _orientation = newOri;
2931 if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f)) 2979 if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
2932 createAMotor(m_angularlock); 2980 createAMotor(m_angularlock);
2933 } 2981 }
2934 if (_position != newPos) 2982 if (_position != newPos)
2935 { 2983 {
2936 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); 2984 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2937 _position = newPos; 2985 _position = newPos;
2938 } 2986 }
2939 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) 2987 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
2940 d.BodyEnable(Body); 2988 d.BodyEnable(Body);
2941 } 2989 }
2942 } 2990 }
2943 else 2991 else
2944 { 2992 {
2945 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position); 2993 // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
2946 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position); 2994 // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
2947 2995
2948 if (prim_geom != IntPtr.Zero) 2996 if (prim_geom != IntPtr.Zero)
2949 { 2997 {
2950 if (newOri != _orientation) 2998 if (newOri != _orientation)
2951 { 2999 {
2952 d.Quaternion myrot = new d.Quaternion(); 3000 d.Quaternion myrot = new d.Quaternion();
2953 myrot.X = newOri.X; 3001 myrot.X = newOri.X;
2954 myrot.Y = newOri.Y; 3002 myrot.Y = newOri.Y;
2955 myrot.Z = newOri.Z; 3003 myrot.Z = newOri.Z;
2956 myrot.W = newOri.W; 3004 myrot.W = newOri.W;
2957 d.GeomSetQuaternion(prim_geom, ref myrot); 3005 d.GeomSetQuaternion(prim_geom, ref myrot);
2958 _orientation = newOri; 3006 _orientation = newOri;
2959 } 3007 }
2960 3008
2961 if (newPos != _position) 3009 if (newPos != _position)
2962 { 3010 {
2963 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z); 3011 d.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
2964 _position = newPos; 3012 _position = newPos;
2965 3013
2966 m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace); 3014 m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, _position, m_targetSpace);
2967 } 3015 }
2968 } 3016 }
2969 } 3017 }
2970 givefakepos--; 3018 givefakepos--;
2971 if (givefakepos < 0) 3019 if (givefakepos < 0)
2972 givefakepos = 0; 3020 givefakepos = 0;
2973 givefakeori--; 3021 givefakeori--;
2974 if (givefakeori < 0) 3022 if (givefakeori < 0)
2975 givefakeori = 0; 3023 givefakeori = 0;
2976 3024
2977 m_softcolide = true; 3025 m_softcolide = true;
2978 resetCollisionAccounting(); 3026 resetCollisionAccounting();
2979 } 3027 }
2980 3028
2981 3029
2982 private void changeDisable(bool disable) 3030 private void changeDisable(bool disable)
2983 { 3031 {
2984 if (disable) 3032 if (disable)
2985 { 3033 {
2986 if (!m_disabled) 3034 if (!m_disabled)
2987 disableBodySoft(); 3035 disableBodySoft();
2988 } 3036 }
2989 else 3037 else
2990 { 3038 {
2991 if (m_disabled) 3039 if (m_disabled)
2992 enableBodySoft(); 3040 enableBodySoft();
2993 } 3041 }
2994 } 3042 }
2995 3043
2996 private void changePhysicsStatus(bool NewStatus) 3044 private void changePhysicsStatus(bool NewStatus)
2997 { 3045 {
2998 CheckDelaySelect(); 3046 CheckDelaySelect();
2999 3047
3000 m_isphysical = NewStatus; 3048 m_isphysical = NewStatus;
3001 3049
3002 if (!childPrim) 3050 if (!childPrim)
3003 { 3051 {
3004 if (NewStatus) 3052 if (NewStatus)
3005 { 3053 {
3006 if (Body == IntPtr.Zero) 3054 if (Body == IntPtr.Zero)
3007 MakeBody(); 3055 MakeBody();
3008 } 3056 }
3009 else 3057 else
3010 { 3058 {
3011 if (Body != IntPtr.Zero) 3059 if (Body != IntPtr.Zero)
3012 { 3060 {
3013 DestroyBody(); 3061 DestroyBody();
3014 } 3062 }
3015 Stop(); 3063 Stop();
3016 } 3064 }
3017 } 3065 }
3018 3066
3019 resetCollisionAccounting(); 3067 resetCollisionAccounting();
3020 } 3068 }
3021 3069
3022 private void changeprimsizeshape() 3070 private void changeprimsizeshape()
3023 { 3071 {
3024 CheckDelaySelect(); 3072 CheckDelaySelect();
3025 3073
3026 OdePrim parent = (OdePrim)_parent; 3074 OdePrim parent = (OdePrim)_parent;
3027 3075
3028 bool chp = childPrim; 3076 bool chp = childPrim;
3029 3077
3030 if (chp) 3078 if (chp)
3031 { 3079 {
3032 if (parent != null) 3080 if (parent != null)
3033 { 3081 {
3034 parent.DestroyBody(); 3082 parent.DestroyBody();
3035 } 3083 }
3036 } 3084 }
3037 else 3085 else
3038 { 3086 {
3039 DestroyBody(); 3087 DestroyBody();
3040 } 3088 }
3041 3089
3042 RemoveGeom(); 3090 RemoveGeom();
3043 3091
3044 // we don't need to do space calculation because the client sends a position update also. 3092 // we don't need to do space calculation because the client sends a position update also.
3045 if (_size.X <= 0) 3093 if (_size.X <= 0)
3046 _size.X = 0.01f; 3094 _size.X = 0.01f;
3047 if (_size.Y <= 0) 3095 if (_size.Y <= 0)
3048 _size.Y = 0.01f; 3096 _size.Y = 0.01f;
3049 if (_size.Z <= 0) 3097 if (_size.Z <= 0)
3050 _size.Z = 0.01f; 3098 _size.Z = 0.01f;
3051 // Construction of new prim 3099 // Construction of new prim
3052 3100
3053 CreateGeom(); 3101 CreateGeom();
3054 3102
3055 if (prim_geom != IntPtr.Zero) 3103 if (prim_geom != IntPtr.Zero)
3056 { 3104 {
3057 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); 3105 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
3058 d.Quaternion myrot = new d.Quaternion(); 3106 d.Quaternion myrot = new d.Quaternion();
3059 myrot.X = _orientation.X; 3107 myrot.X = _orientation.X;
3060 myrot.Y = _orientation.Y; 3108 myrot.Y = _orientation.Y;
3061 myrot.Z = _orientation.Z; 3109 myrot.Z = _orientation.Z;
3062 myrot.W = _orientation.W; 3110 myrot.W = _orientation.W;
3063 d.GeomSetQuaternion(prim_geom, ref myrot); 3111 d.GeomSetQuaternion(prim_geom, ref myrot);
3064 } 3112 }
3065 3113
3066 if (chp) 3114 if (chp)
3067 { 3115 {
3068 if (parent != null) 3116 if (parent != null)
3069 { 3117 {
3070 parent.MakeBody(); 3118 parent.MakeBody();
3071 } 3119 }
3072 } 3120 }
3073 else 3121 else
3074 MakeBody(); 3122 MakeBody();
3075 3123
3076 m_softcolide = true; 3124 m_softcolide = true;
3077 resetCollisionAccounting(); 3125 resetCollisionAccounting();
3078 } 3126 }
3079 3127
3080 private void changeSize(Vector3 newSize) 3128 private void changeSize(Vector3 newSize)
3081 { 3129 {
3082 _size = newSize; 3130 _size = newSize;
3083 changeprimsizeshape(); 3131 changeprimsizeshape();
3084 } 3132 }
3085 3133
3086 private void changeShape(PrimitiveBaseShape newShape) 3134 private void changeShape(PrimitiveBaseShape newShape)
3087 { 3135 {
3088 if(newShape != null) 3136 if(newShape != null)
3089 _pbs = newShape; 3137 _pbs = newShape;
3090 changeprimsizeshape(); 3138 changeprimsizeshape();
3091 } 3139 }
3092 3140
3093 private void changeFloatOnWater(bool newval) 3141 private void changeFloatOnWater(bool newval)
3094 { 3142 {
3095 m_collidesWater = newval; 3143 m_collidesWater = newval;
3096 3144
3097 if (prim_geom != IntPtr.Zero && !m_isphantom) 3145 if (prim_geom != IntPtr.Zero && !m_isphantom)
3098 { 3146 {
3099 if (m_collidesWater) 3147 if (m_collidesWater)
3100 { 3148 {
3101 m_collisionFlags |= CollisionCategories.Water; 3149 m_collisionFlags |= CollisionCategories.Water;
3102 } 3150 }
3103 else 3151 else
3104 { 3152 {
3105 m_collisionFlags &= ~CollisionCategories.Water; 3153 m_collisionFlags &= ~CollisionCategories.Water;
3106 } 3154 }
3107 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags); 3155 d.GeomSetCollideBits(prim_geom, (int)m_collisionFlags);
3108 } 3156 }
3109 } 3157 }
3110 3158
3111 private void changeSetTorque(Vector3 newtorque) 3159 private void changeSetTorque(Vector3 newtorque)
3112 { 3160 {
3113 if (!m_isSelected) 3161 if (!m_isSelected)
3114 { 3162 {
3115 if (m_isphysical && Body != IntPtr.Zero) 3163 if (m_isphysical && Body != IntPtr.Zero)
3116 { 3164 {
3117 if (m_disabled) 3165 if (m_disabled)
3118 enableBodySoft(); 3166 enableBodySoft();
3119 else if (!d.BodyIsEnabled(Body)) 3167 else if (!d.BodyIsEnabled(Body))
3120 d.BodyEnable(Body); 3168 d.BodyEnable(Body);
3121 3169
3122 } 3170 }
3123 _torque = newtorque; 3171 _torque = newtorque;
3124 } 3172 }
3125 } 3173 }
3126 3174
3127 private void changeForce(Vector3 force) 3175 private void changeForce(Vector3 force)
3128 { 3176 {
3129 m_force = force; 3177 m_force = force;
3130 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body)) 3178 if (Body != IntPtr.Zero && !d.BodyIsEnabled(Body))
3131 d.BodyEnable(Body); 3179 d.BodyEnable(Body);
3132 } 3180 }
3133 3181
3134 private void changeAddForce(Vector3 force) 3182 private void changeAddForce(Vector3 force)
3135 { 3183 {
3136 m_forceacc += force; 3184 m_forceacc += force;
3137 if (!m_isSelected) 3185 if (!m_isSelected)
3138 { 3186 {
3139 lock (this) 3187 lock (this)
3140 { 3188 {
3141 //m_log.Info("[PHYSICS]: dequeing forcelist"); 3189 //m_log.Info("[PHYSICS]: dequeing forcelist");
3142 if (m_isphysical && Body != IntPtr.Zero) 3190 if (m_isphysical && Body != IntPtr.Zero)
3143 { 3191 {
3144 if (m_disabled) 3192 if (m_disabled)
3145 enableBodySoft(); 3193 enableBodySoft();
3146 else if (!d.BodyIsEnabled(Body)) 3194 else if (!d.BodyIsEnabled(Body))
3147 d.BodyEnable(Body); 3195 d.BodyEnable(Body);
3148 } 3196 }
3149 } 3197 }
3150 3198
3151 m_collisionscore = 0; 3199 m_collisionscore = 0;
3152 } 3200 }
3153 } 3201 }
3154 3202
3155 private void changeAddAngularForce(Vector3 aforce) 3203 private void changeAddAngularForce(Vector3 aforce)
3156 { 3204 {
3157 m_angularForceacc += aforce; 3205 m_angularForceacc += aforce;
3158 if (!m_isSelected) 3206 if (!m_isSelected)
3159 { 3207 {
3160 lock (this) 3208 lock (this)
3161 { 3209 {
3162 if (m_isphysical && Body != IntPtr.Zero) 3210 if (m_isphysical && Body != IntPtr.Zero)
3163 { 3211 {
3164 if (m_disabled) 3212 if (m_disabled)
3165 enableBodySoft(); 3213 enableBodySoft();
3166 else if (!d.BodyIsEnabled(Body)) 3214 else if (!d.BodyIsEnabled(Body))
3167 d.BodyEnable(Body); 3215 d.BodyEnable(Body);
3168 } 3216 }
3169 } 3217 }
3170 m_collisionscore = 0; 3218 m_collisionscore = 0;
3171 } 3219 }
3172 } 3220 }
3173 3221
3174 private void changevelocity(Vector3 newVel) 3222 private void changevelocity(Vector3 newVel)
3175 { 3223 {
3176 if (!m_isSelected) 3224 if (!m_isSelected)
3177 { 3225 {
3178 if (Body != IntPtr.Zero) 3226 if (Body != IntPtr.Zero)
3179 { 3227 {
3180 if (m_disabled) 3228 if (m_disabled)
3181 enableBodySoft(); 3229 enableBodySoft();
3182 else if (!d.BodyIsEnabled(Body)) 3230 else if (!d.BodyIsEnabled(Body))
3183 d.BodyEnable(Body); 3231 d.BodyEnable(Body);
3184 3232
3185 d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z); 3233 d.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z);
3186 } 3234 }
3187 //resetCollisionAccounting(); 3235 //resetCollisionAccounting();
3188 } 3236 }
3189 _velocity = newVel; 3237 _velocity = newVel;
3190 } 3238 }
3191 3239
3192 private void changeVolumedetetion(bool newVolDtc) 3240 private void changeVolumedetetion(bool newVolDtc)
3193 { 3241 {
3194 m_isVolumeDetect = newVolDtc; 3242 m_isVolumeDetect = newVolDtc;
3195 } 3243 }
3196 3244
3197 protected void changeBuilding(bool newbuilding) 3245 protected void changeBuilding(bool newbuilding)
3198 { 3246 {
3199 if ((bool)newbuilding) 3247 if ((bool)newbuilding)
3200 { 3248 {
3201 m_building = true; 3249 m_building = true;
3202 if (!childPrim) 3250 if (!childPrim)
3203 DestroyBody(); 3251 DestroyBody();
3204 } 3252 }
3205 else 3253 else
3206 { 3254 {
3207 m_building = false; 3255 m_building = false;
3208 CheckDelaySelect(); 3256 CheckDelaySelect();
3209 if (!childPrim) 3257 if (!childPrim)
3210 MakeBody(); 3258 MakeBody();
3211 } 3259 }
3212 if (!childPrim && childrenPrim.Count > 0) 3260 if (!childPrim && childrenPrim.Count > 0)
3213 { 3261 {
3214 foreach (OdePrim prm in childrenPrim) 3262 foreach (OdePrim prm in childrenPrim)
3215 prm.changeBuilding(m_building); // call directly 3263 prm.changeBuilding(m_building); // call directly
3216 } 3264 }
3217 } 3265 }
3218 3266
3219 public void changeSetVehicle(VehicleData vdata) 3267 public void changeSetVehicle(VehicleData vdata)
3220 { 3268 {
3221 if (m_vehicle == null) 3269 if (m_vehicle == null)
3222 m_vehicle = new ODEDynamics(this); 3270 m_vehicle = new ODEDynamics(this);
3223 m_vehicle.DoSetVehicle(vdata); 3271 m_vehicle.DoSetVehicle(vdata);
3224 } 3272 }
3225 private void changeVehicleType(int value) 3273 private void changeVehicleType(int value)
3226 { 3274 {
3227 if (value == (int)Vehicle.TYPE_NONE) 3275 if (value == (int)Vehicle.TYPE_NONE)
3228 { 3276 {
3229 if (m_vehicle != null) 3277 if (m_vehicle != null)
3230 m_vehicle = null; 3278 m_vehicle = null;
3231 } 3279 }
3232 else 3280 else
3233 { 3281 {
3234 if (m_vehicle == null) 3282 if (m_vehicle == null)
3235 m_vehicle = new ODEDynamics(this); 3283 m_vehicle = new ODEDynamics(this);
3236 3284
3237 m_vehicle.ProcessTypeChange((Vehicle)value); 3285 m_vehicle.ProcessTypeChange((Vehicle)value);
3238 } 3286 }
3239 } 3287 }
3240 3288
3241 private void changeVehicleFloatParam(strVehicleFloatParam fp) 3289 private void changeVehicleFloatParam(strVehicleFloatParam fp)
3242 { 3290 {
3243 if (m_vehicle == null) 3291 if (m_vehicle == null)
3244 return; 3292 return;
3245 3293
3246 m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value); 3294 m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value);
3247 } 3295 }
3248 3296
3249 private void changeVehicleVectorParam(strVehicleVectorParam vp) 3297 private void changeVehicleVectorParam(strVehicleVectorParam vp)
3250 { 3298 {
3251 if (m_vehicle == null) 3299 if (m_vehicle == null)
3252 return; 3300 return;
3253 m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value); 3301 m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value);
3254 } 3302 }
3255 3303
3256 private void changeVehicleRotationParam(strVehicleQuatParam qp) 3304 private void changeVehicleRotationParam(strVehicleQuatParam qp)
3257 { 3305 {
3258 if (m_vehicle == null) 3306 if (m_vehicle == null)
3259 return; 3307 return;
3260 m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value); 3308 m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value);
3261 } 3309 }
3262 3310
3263 private void changeVehicleFlags(strVehicleBoolParam bp) 3311 private void changeVehicleFlags(strVehicleBoolParam bp)
3264 { 3312 {
3265 if (m_vehicle == null) 3313 if (m_vehicle == null)
3266 return; 3314 return;
3267 m_vehicle.ProcessVehicleFlags(bp.param, bp.value); 3315 m_vehicle.ProcessVehicleFlags(bp.param, bp.value);
3268 } 3316 }
3269 3317
3270 #endregion 3318 #endregion
3271 3319
3272 public void Move() 3320 public void Move()
3273 { 3321 {
3274 if (!childPrim && m_isphysical && Body != IntPtr.Zero && 3322 if (!childPrim && m_isphysical && Body != IntPtr.Zero &&
3275 !m_disabled && !m_isSelected && d.BodyIsEnabled(Body) && !m_building && !m_outbounds) 3323 !m_disabled && !m_isSelected && d.BodyIsEnabled(Body) && !m_building && !m_outbounds)
3276 // !m_disabled && !m_isSelected && !m_building && !m_outbounds) 3324 // !m_disabled && !m_isSelected && !m_building && !m_outbounds)
3277 { 3325 {
3278// if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009 3326// if (!d.BodyIsEnabled(Body)) d.BodyEnable(Body); // KF add 161009
3279 3327
3280 float timestep = _parent_scene.ODE_STEPSIZE; 3328 float timestep = _parent_scene.ODE_STEPSIZE;
3281 3329
3282 // check outside region 3330 // check outside region
3283 d.Vector3 lpos; 3331 d.Vector3 lpos;
3284 d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator 3332 d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator
3285 3333
3286 if (lpos.Z < -100 || lpos.Z > 100000f) 3334 if (lpos.Z < -100 || lpos.Z > 100000f)
3287 { 3335 {
3288 m_outbounds = true; 3336 m_outbounds = true;
3289 3337
3290 lpos.Z = Util.Clip(lpos.Z, -100f, 100000f); 3338 lpos.Z = Util.Clip(lpos.Z, -100f, 100000f);
3291 _acceleration.X = 0; 3339 _acceleration.X = 0;
3292 _acceleration.Y = 0; 3340 _acceleration.Y = 0;
3293 _acceleration.Z = 0; 3341 _acceleration.Z = 0;
3294 3342
3295 _velocity.X = 0; 3343 _velocity.X = 0;
3296 _velocity.Y = 0; 3344 _velocity.Y = 0;
3297 _velocity.Z = 0; 3345 _velocity.Z = 0;
3298 m_rotationalVelocity.X = 0; 3346 m_rotationalVelocity.X = 0;
3299 m_rotationalVelocity.Y = 0; 3347 m_rotationalVelocity.Y = 0;
3300 m_rotationalVelocity.Z = 0; 3348 m_rotationalVelocity.Z = 0;
3301 3349
3302 d.BodySetLinearVel(Body, 0, 0, 0); // stop it 3350 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
3303 d.BodySetAngularVel(Body, 0, 0, 0); // stop it 3351 d.BodySetAngularVel(Body, 0, 0, 0); // stop it
3304 d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere 3352 d.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere
3305 m_lastposition = _position; 3353 m_lastposition = _position;
3306 m_lastorientation = _orientation; 3354 m_lastorientation = _orientation;
3307 3355
3308 base.RequestPhysicsterseUpdate(); 3356 base.RequestPhysicsterseUpdate();
3309 3357
3310 m_throttleUpdates = false; 3358 m_throttleUpdates = false;
3311 throttleCounter = 0; 3359 throttleCounter = 0;
3312 _zeroFlag = true; 3360 _zeroFlag = true;
3313 3361
3314 disableBodySoft(); // disable it and colisions 3362 disableBodySoft(); // disable it and colisions
3315 base.RaiseOutOfBounds(_position); 3363 base.RaiseOutOfBounds(_position);
3316 return; 3364 return;
3317 } 3365 }
3318 3366
3319 if (lpos.X < 0f) 3367 if (lpos.X < 0f)
3320 { 3368 {
3321 _position.X = Util.Clip(lpos.X, -2f, -0.1f); 3369 _position.X = Util.Clip(lpos.X, -2f, -0.1f);
3322 m_outbounds = true; 3370 m_outbounds = true;
3323 } 3371 }
3324 else if(lpos.X > _parent_scene.WorldExtents.X) 3372 else if(lpos.X > _parent_scene.WorldExtents.X)
3325 { 3373 {
3326 _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f); 3374 _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f);
3327 m_outbounds = true; 3375 m_outbounds = true;
3328 } 3376 }
3329 if (lpos.Y < 0f) 3377 if (lpos.Y < 0f)
3330 { 3378 {
3331 _position.Y = Util.Clip(lpos.Y, -2f, -0.1f); 3379 _position.Y = Util.Clip(lpos.Y, -2f, -0.1f);
3332 m_outbounds = true; 3380 m_outbounds = true;
3333 } 3381 }
3334 else if(lpos.Y > _parent_scene.WorldExtents.Y) 3382 else if(lpos.Y > _parent_scene.WorldExtents.Y)
3335 { 3383 {
3336 _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f); 3384 _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f);
3337 m_outbounds = true; 3385 m_outbounds = true;
3338 } 3386 }
3339 3387
3340 if(m_outbounds) 3388 if(m_outbounds)
3341 { 3389 {
3342 m_lastposition = _position; 3390 m_lastposition = _position;
3343 m_lastorientation = _orientation; 3391 m_lastorientation = _orientation;
3344 3392
3345 d.Vector3 dtmp = d.BodyGetAngularVel(Body); 3393 d.Vector3 dtmp = d.BodyGetAngularVel(Body);
3346 m_rotationalVelocity.X = dtmp.X; 3394 m_rotationalVelocity.X = dtmp.X;
3347 m_rotationalVelocity.Y = dtmp.Y; 3395 m_rotationalVelocity.Y = dtmp.Y;
3348 m_rotationalVelocity.Z = dtmp.Z; 3396 m_rotationalVelocity.Z = dtmp.Z;
3349 3397
3350 dtmp = d.BodyGetLinearVel(Body); 3398 dtmp = d.BodyGetLinearVel(Body);
3351 _velocity.X = dtmp.X; 3399 _velocity.X = dtmp.X;
3352 _velocity.Y = dtmp.Y; 3400 _velocity.Y = dtmp.Y;
3353 _velocity.Z = dtmp.Z; 3401 _velocity.Z = dtmp.Z;
3354 3402
3355 d.BodySetLinearVel(Body, 0, 0, 0); // stop it 3403 d.BodySetLinearVel(Body, 0, 0, 0); // stop it
3356 d.BodySetAngularVel(Body, 0, 0, 0); 3404 d.BodySetAngularVel(Body, 0, 0, 0);
3357 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z); 3405 d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
3358 disableBodySoft(); // stop collisions 3406 disableBodySoft(); // stop collisions
3359 base.RequestPhysicsterseUpdate(); 3407 base.RequestPhysicsterseUpdate();
3360 return; 3408 return;
3361 } 3409 }
3362 3410
3363 3411
3364 float fx = 0; 3412 float fx = 0;
3365 float fy = 0; 3413 float fy = 0;
3366 float fz = 0; 3414 float fz = 0;
3367 3415
3368 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) 3416 if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
3369 { 3417 {
3370 // 'VEHICLES' are dealt with in ODEDynamics.cs 3418 // 'VEHICLES' are dealt with in ODEDynamics.cs
3371 m_vehicle.Step(); 3419 m_vehicle.Step();
3372 } 3420 }
3373 else 3421 else
3374 { 3422 {
3375 float m_mass = _mass; 3423 float m_mass = _mass;
3376 3424
3377 // fz = 0f; 3425 // fz = 0f;
3378 //m_log.Info(m_collisionFlags.ToString()); 3426 //m_log.Info(m_collisionFlags.ToString());
3379 if (m_usePID) 3427 if (m_usePID)
3380 { 3428 {
3381 3429
3382 // If the PID Controller isn't active then we set our force 3430 // If the PID Controller isn't active then we set our force
3383 // calculating base velocity to the current position 3431 // calculating base velocity to the current position
3384 3432
3385 if ((m_PIDTau < 1) && (m_PIDTau != 0)) 3433 if ((m_PIDTau < 1) && (m_PIDTau != 0))
3386 { 3434 {
3387 //PID_G = PID_G / m_PIDTau; 3435 //PID_G = PID_G / m_PIDTau;
3388 m_PIDTau = 1; 3436 m_PIDTau = 1;
3389 } 3437 }
3390 3438
3391 if ((PID_G - m_PIDTau) <= 0) 3439 if ((PID_G - m_PIDTau) <= 0)
3392 { 3440 {
3393 PID_G = m_PIDTau + 1; 3441 PID_G = m_PIDTau + 1;
3394 } 3442 }
3395 3443
3396 d.Vector3 vel = d.BodyGetLinearVel(Body); 3444 d.Vector3 vel = d.BodyGetLinearVel(Body);
3397 d.Vector3 pos = d.BodyGetPosition(Body); 3445 d.Vector3 pos = d.BodyGetPosition(Body);
3398 _target_velocity = 3446 _target_velocity =
3399 new Vector3( 3447 new Vector3(
3400 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep), 3448 (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
3401 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep), 3449 (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
3402 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep) 3450 (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
3403 ); 3451 );
3404 3452
3405 // if velocity is zero, use position control; otherwise, velocity control 3453 // if velocity is zero, use position control; otherwise, velocity control
3406 3454
3407 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) 3455 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
3408 { 3456 {
3409 // keep track of where we stopped. No more slippin' & slidin' 3457 // keep track of where we stopped. No more slippin' & slidin'
3410 3458
3411 // We only want to deactivate the PID Controller if we think we want to have our surrogate 3459 // We only want to deactivate the PID Controller if we think we want to have our surrogate
3412 // react to the physics scene by moving it's position. 3460 // react to the physics scene by moving it's position.
3413 // Avatar to Avatar collisions 3461 // Avatar to Avatar collisions
3414 // Prim to avatar collisions 3462 // Prim to avatar collisions
3415 3463
3416 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2); 3464 //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
3417 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2); 3465 //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
3418 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P; 3466 //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
3419 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z); 3467 d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
3420 d.BodySetLinearVel(Body, 0, 0, 0); 3468 d.BodySetLinearVel(Body, 0, 0, 0);
3421 d.BodyAddForce(Body, 0, 0, fz); 3469 d.BodyAddForce(Body, 0, 0, fz);
3422 return; 3470 return;
3423 } 3471 }
3424 else 3472 else
3425 { 3473 {
3426 _zeroFlag = false; 3474 _zeroFlag = false;
3427 3475
3428 // We're flying and colliding with something 3476 // We're flying and colliding with something
3429 fx = ((_target_velocity.X) - vel.X) * (PID_D); 3477 fx = ((_target_velocity.X) - vel.X) * (PID_D);
3430 fy = ((_target_velocity.Y) - vel.Y) * (PID_D); 3478 fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
3431 3479
3432 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P; 3480 // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
3433 3481
3434 fz = ((_target_velocity.Z - vel.Z) * (PID_D)); 3482 fz = ((_target_velocity.Z - vel.Z) * (PID_D));
3435 } 3483 }
3436 } // end if (m_usePID) 3484 } // end if (m_usePID)
3437 3485
3438 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller 3486 // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
3439 else if (m_useHoverPID) 3487 else if (m_useHoverPID)
3440 { 3488 {
3441 //Console.WriteLine("Hover " + Name); 3489 //Console.WriteLine("Hover " + Name);
3442 3490
3443 // If we're using the PID controller, then we have no gravity 3491 // If we're using the PID controller, then we have no gravity
3444 3492
3445 // no lock; for now it's only called from within Simulate() 3493 // no lock; for now it's only called from within Simulate()
3446 3494
3447 // If the PID Controller isn't active then we set our force 3495 // If the PID Controller isn't active then we set our force
3448 // calculating base velocity to the current position 3496 // calculating base velocity to the current position
3449 3497
3450 if ((m_PIDTau < 1)) 3498 if ((m_PIDTau < 1))
3451 { 3499 {
3452 PID_G = PID_G / m_PIDTau; 3500 PID_G = PID_G / m_PIDTau;
3453 } 3501 }
3454 3502
3455 if ((PID_G - m_PIDTau) <= 0) 3503 if ((PID_G - m_PIDTau) <= 0)
3456 { 3504 {
3457 PID_G = m_PIDTau + 1; 3505 PID_G = m_PIDTau + 1;
3458 } 3506 }
3459 3507
3460 // Where are we, and where are we headed? 3508 // Where are we, and where are we headed?
3461 d.Vector3 pos = d.BodyGetPosition(Body); 3509 d.Vector3 pos = d.BodyGetPosition(Body);
3462 d.Vector3 vel = d.BodyGetLinearVel(Body); 3510 d.Vector3 vel = d.BodyGetLinearVel(Body);
3463 3511
3464 // Non-Vehicles have a limited set of Hover options. 3512 // Non-Vehicles have a limited set of Hover options.
3465 // determine what our target height really is based on HoverType 3513 // determine what our target height really is based on HoverType
3466 switch (m_PIDHoverType) 3514 switch (m_PIDHoverType)
3467 { 3515 {
3468 case PIDHoverType.Ground: 3516 case PIDHoverType.Ground:
3469 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); 3517 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3470 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; 3518 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3471 break; 3519 break;
3472 case PIDHoverType.GroundAndWater: 3520 case PIDHoverType.GroundAndWater:
3473 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y); 3521 m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
3474 m_waterHeight = _parent_scene.GetWaterLevel(); 3522 m_waterHeight = _parent_scene.GetWaterLevel();
3475 if (m_groundHeight > m_waterHeight) 3523 if (m_groundHeight > m_waterHeight)
3476 { 3524 {
3477 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight; 3525 m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
3478 } 3526 }
3479 else 3527 else
3480 { 3528 {
3481 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight; 3529 m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
3482 } 3530 }
3483 break; 3531 break;
3484 3532
3485 } // end switch (m_PIDHoverType) 3533 } // end switch (m_PIDHoverType)
3486 3534
3487 3535
3488 _target_velocity = 3536 _target_velocity =
3489 new Vector3(0.0f, 0.0f, 3537 new Vector3(0.0f, 0.0f,
3490 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep) 3538 (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
3491 ); 3539 );
3492 3540
3493 // if velocity is zero, use position control; otherwise, velocity control 3541 // if velocity is zero, use position control; otherwise, velocity control
3494 3542
3495 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f)) 3543 if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
3496 { 3544 {
3497 // keep track of where we stopped. No more slippin' & slidin' 3545 // keep track of where we stopped. No more slippin' & slidin'
3498 3546
3499 // We only want to deactivate the PID Controller if we think we want to have our surrogate 3547 // We only want to deactivate the PID Controller if we think we want to have our surrogate
3500 // react to the physics scene by moving it's position. 3548 // react to the physics scene by moving it's position.
3501 // Avatar to Avatar collisions 3549 // Avatar to Avatar collisions
3502 // Prim to avatar collisions 3550 // Prim to avatar collisions
3503 3551
3504 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight); 3552 d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
3505 d.BodySetLinearVel(Body, vel.X, vel.Y, 0); 3553 d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
3506 // ? d.BodyAddForce(Body, 0, 0, fz); 3554 // ? d.BodyAddForce(Body, 0, 0, fz);
3507 return; 3555 return;
3508 } 3556 }
3509 else 3557 else
3510 { 3558 {
3511 _zeroFlag = false; 3559 _zeroFlag = false;
3512 3560
3513 // We're flying and colliding with something 3561 // We're flying and colliding with something
3514 fz = ((_target_velocity.Z - vel.Z) * (PID_D)); 3562 fz = ((_target_velocity.Z - vel.Z) * (PID_D));
3515 } 3563 }
3516 } 3564 }
3517 else 3565 else
3518 { 3566 {
3519 float b = (1.0f - m_buoyancy); 3567 float b = (1.0f - m_buoyancy);
3520 fx = _parent_scene.gravityx * b; 3568 fx = _parent_scene.gravityx * b;
3521 fy = _parent_scene.gravityy * b; 3569 fy = _parent_scene.gravityy * b;
3522 fz = _parent_scene.gravityz * b; 3570 fz = _parent_scene.gravityz * b;
3523 } 3571 }
3524 3572
3525 fx *= m_mass; 3573 fx *= m_mass;
3526 fy *= m_mass; 3574 fy *= m_mass;
3527 fz *= m_mass; 3575 fz *= m_mass;
3528 3576
3529 // constant force 3577 // constant force
3530 fx += m_force.X; 3578 fx += m_force.X;
3531 fy += m_force.Y; 3579 fy += m_force.Y;
3532 fz += m_force.Z; 3580 fz += m_force.Z;
3533 3581
3534 fx += m_forceacc.X; 3582 fx += m_forceacc.X;
3535 fy += m_forceacc.Y; 3583 fy += m_forceacc.Y;
3536 fz += m_forceacc.Z; 3584 fz += m_forceacc.Z;
3537 3585
3538 m_forceacc = Vector3.Zero; 3586 m_forceacc = Vector3.Zero;
3539 3587
3540 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString()); 3588 //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
3541 if (fx != 0 || fy != 0 || fz != 0) 3589 if (fx != 0 || fy != 0 || fz != 0)
3542 { 3590 {
3543 d.BodyAddForce(Body, fx, fy, fz); 3591 d.BodyAddForce(Body, fx, fy, fz);
3544 //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz); 3592 //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
3545 } 3593 }
3546 3594
3547 Vector3 trq; 3595 Vector3 trq;
3548 3596
3549 trq = _torque; 3597 trq = _torque;
3550 trq += m_angularForceacc; 3598 trq += m_angularForceacc;
3551 m_angularForceacc = Vector3.Zero; 3599 m_angularForceacc = Vector3.Zero;
3552 if (trq.X != 0 || trq.Y != 0 || trq.Z != 0) 3600 if (trq.X != 0 || trq.Y != 0 || trq.Z != 0)
3553 { 3601 {
3554 d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z); 3602 d.BodyAddTorque(Body, trq.X, trq.Y, trq.Z);
3555 } 3603 }
3556 3604
3557 } 3605 }
3558 } 3606 }
3559 else 3607 else
3560 { // is not physical, or is not a body or is selected 3608 { // is not physical, or is not a body or is selected
3561 // _zeroPosition = d.BodyGetPosition(Body); 3609 // _zeroPosition = d.BodyGetPosition(Body);
3562 return; 3610 return;
3563 //Console.WriteLine("Nothing " + Name); 3611 //Console.WriteLine("Nothing " + Name);
3564 3612
3565 } 3613 }
3566 } 3614 }
3567 3615
3568 3616
3569 public void UpdatePositionAndVelocity(float simulatedtime) 3617 public void UpdatePositionAndVelocity(float simulatedtime)
3570 { 3618 {
3571 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit! 3619 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
3572 if (_parent == null && !m_disabled && !m_building && !m_outbounds) 3620 if (_parent == null && !m_disabled && !m_building && !m_outbounds)
3573 { 3621 {
3574 if (Body != IntPtr.Zero) 3622 if (Body != IntPtr.Zero)
3575 { 3623 {
3576 Vector3 pv = Vector3.Zero; 3624 Vector3 pv = Vector3.Zero;
3577 bool lastZeroFlag = _zeroFlag; 3625 bool lastZeroFlag = _zeroFlag;
3578 3626
3579 d.Vector3 lpos; 3627 d.Vector3 lpos;
3580 d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator 3628 d.GeomCopyPosition(prim_geom, out lpos); // root position that is seem by rest of simulator
3581 3629
3582 3630
3583 d.Quaternion ori; 3631 d.Quaternion ori;
3584 d.GeomCopyQuaternion(prim_geom, out ori); 3632 d.GeomCopyQuaternion(prim_geom, out ori);
3585 d.Vector3 vel = d.BodyGetLinearVel(Body); 3633 d.Vector3 vel = d.BodyGetLinearVel(Body);
3586 d.Vector3 rotvel = d.BodyGetAngularVel(Body); 3634 d.Vector3 rotvel = d.BodyGetAngularVel(Body);
3587 3635
3588 if ((Math.Abs(m_lastposition.X - lpos.X) < 0.01) 3636 if ((Math.Abs(m_lastposition.X - lpos.X) < 0.01)
3589 && (Math.Abs(m_lastposition.Y - lpos.Y) < 0.01) 3637 && (Math.Abs(m_lastposition.Y - lpos.Y) < 0.01)
3590 && (Math.Abs(m_lastposition.Z - lpos.Z) < 0.01) 3638 && (Math.Abs(m_lastposition.Z - lpos.Z) < 0.01)
3591 && (Math.Abs(m_lastorientation.X - ori.X) < 0.0001) 3639 && (Math.Abs(m_lastorientation.X - ori.X) < 0.0001)
3592 && (Math.Abs(m_lastorientation.Y - ori.Y) < 0.0001) 3640 && (Math.Abs(m_lastorientation.Y - ori.Y) < 0.0001)
3593 && (Math.Abs(m_lastorientation.Z - ori.Z) < 0.0001) 3641 && (Math.Abs(m_lastorientation.Z - ori.Z) < 0.0001)
3594 ) 3642 )
3595 { 3643 {
3596 _zeroFlag = true; 3644 _zeroFlag = true;
3597 //Console.WriteLine("ZFT 2"); 3645 //Console.WriteLine("ZFT 2");
3598 m_throttleUpdates = false; 3646 m_throttleUpdates = false;
3599 } 3647 }
3600 else 3648 else
3601 { 3649 {
3602 //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString()); 3650 //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
3603 _zeroFlag = false; 3651 _zeroFlag = false;
3604 m_lastUpdateSent = false; 3652 m_lastUpdateSent = false;
3605 //m_throttleUpdates = false; 3653 //m_throttleUpdates = false;
3606 } 3654 }
3607 3655
3608 if (_zeroFlag) 3656 if (_zeroFlag)
3609 { 3657 {
3610 m_lastposition = _position; 3658 m_lastposition = _position;
3611 m_lastorientation = _orientation; 3659 m_lastorientation = _orientation;
3612 3660
3613 _velocity.X = 0.0f; 3661 _velocity.X = 0.0f;
3614 _velocity.Y = 0.0f; 3662 _velocity.Y = 0.0f;
3615 _velocity.Z = 0.0f; 3663 _velocity.Z = 0.0f;
3616 3664
3617 _acceleration.X = 0; 3665 _acceleration.X = 0;
3618 _acceleration.Y = 0; 3666 _acceleration.Y = 0;
3619 _acceleration.Z = 0; 3667 _acceleration.Z = 0;
3620 3668
3621 m_rotationalVelocity.X = 0; 3669 m_rotationalVelocity.X = 0;
3622 m_rotationalVelocity.Y = 0; 3670 m_rotationalVelocity.Y = 0;
3623 m_rotationalVelocity.Z = 0; 3671 m_rotationalVelocity.Z = 0;
3624 if (!m_lastUpdateSent) 3672 if (!m_lastUpdateSent)
3625 { 3673 {
3626 m_throttleUpdates = false; 3674 m_throttleUpdates = false;
3627 throttleCounter = 0; 3675 throttleCounter = 0;
3628 m_rotationalVelocity = pv; 3676 m_rotationalVelocity = pv;
3629 3677
3630 base.RequestPhysicsterseUpdate(); 3678 base.RequestPhysicsterseUpdate();
3631 3679
3632 m_lastUpdateSent = true; 3680 m_lastUpdateSent = true;
3633 } 3681 }
3634 } 3682 }
3635 else 3683 else
3636 { 3684 {
3637 if (lastZeroFlag != _zeroFlag) 3685 if (lastZeroFlag != _zeroFlag)
3638 { 3686 {
3639 base.RequestPhysicsterseUpdate(); 3687 base.RequestPhysicsterseUpdate();
3640 } 3688 }
3641 3689
3642 m_lastVelocity = _velocity; 3690 m_lastVelocity = _velocity;
3643 3691
3644 _position.X = lpos.X; 3692 _position.X = lpos.X;
3645 _position.Y = lpos.Y; 3693 _position.Y = lpos.Y;
3646 _position.Z = lpos.Z; 3694 _position.Z = lpos.Z;
3647 3695
3648 _velocity.X = vel.X; 3696 _velocity.X = vel.X;
3649 _velocity.Y = vel.Y; 3697 _velocity.Y = vel.Y;
3650 _velocity.Z = vel.Z; 3698 _velocity.Z = vel.Z;
3651 3699
3652 _orientation.X = ori.X; 3700 _orientation.X = ori.X;
3653 _orientation.Y = ori.Y; 3701 _orientation.Y = ori.Y;
3654 _orientation.Z = ori.Z; 3702 _orientation.Z = ori.Z;
3655 _orientation.W = ori.W; 3703 _orientation.W = ori.W;
3656 3704
3657 _acceleration = ((_velocity - m_lastVelocity) / simulatedtime); 3705 _acceleration = ((_velocity - m_lastVelocity) / simulatedtime);
3658 3706
3659 if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f)) 3707 if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f))
3660 { 3708 {
3661 m_rotationalVelocity = pv; 3709 m_rotationalVelocity = pv;
3662 } 3710 }
3663 else 3711 else
3664 { 3712 {
3665 m_rotationalVelocity.X = rotvel.X; 3713 m_rotationalVelocity.X = rotvel.X;
3666 m_rotationalVelocity.Y = rotvel.Y; 3714 m_rotationalVelocity.Y = rotvel.Y;
3667 m_rotationalVelocity.Z = rotvel.Z; 3715 m_rotationalVelocity.Z = rotvel.Z;
3668 } 3716 }
3669 3717
3670 m_lastUpdateSent = false; 3718 m_lastUpdateSent = false;
3671 if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate) 3719 if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
3672 { 3720 {
3673 m_lastposition = _position; 3721 m_lastposition = _position;
3674 m_lastorientation = _orientation; 3722 m_lastorientation = _orientation;
3675 base.RequestPhysicsterseUpdate(); 3723 base.RequestPhysicsterseUpdate();
3676 } 3724 }
3677 else 3725 else
3678 { 3726 {
3679 throttleCounter++; 3727 throttleCounter++;
3680 } 3728 }
3681 } 3729 }
3682 } 3730 }
3683 else if (!m_lastUpdateSent || !_zeroFlag) 3731 else if (!m_lastUpdateSent || !_zeroFlag)
3684 { 3732 {
3685 // Not a body.. so Make sure the client isn't interpolating 3733 // Not a body.. so Make sure the client isn't interpolating
3686 _velocity.X = 0; 3734 _velocity.X = 0;
3687 _velocity.Y = 0; 3735 _velocity.Y = 0;
3688 _velocity.Z = 0; 3736 _velocity.Z = 0;
3689 3737
3690 _acceleration.X = 0; 3738 _acceleration.X = 0;
3691 _acceleration.Y = 0; 3739 _acceleration.Y = 0;
3692 _acceleration.Z = 0; 3740 _acceleration.Z = 0;
3693 3741
3694 m_rotationalVelocity.X = 0; 3742 m_rotationalVelocity.X = 0;
3695 m_rotationalVelocity.Y = 0; 3743 m_rotationalVelocity.Y = 0;
3696 m_rotationalVelocity.Z = 0; 3744 m_rotationalVelocity.Z = 0;
3697 _zeroFlag = true; 3745 _zeroFlag = true;
3698 3746
3699 if (!m_lastUpdateSent) 3747 if (!m_lastUpdateSent)
3700 { 3748 {
3701 m_throttleUpdates = false; 3749 m_throttleUpdates = false;
3702 throttleCounter = 0; 3750 throttleCounter = 0;
3703 3751
3704 base.RequestPhysicsterseUpdate(); 3752 base.RequestPhysicsterseUpdate();
3705 3753
3706 m_lastUpdateSent = true; 3754 m_lastUpdateSent = true;
3707 } 3755 }
3708 } 3756 }
3709 } 3757 }
3710 } 3758 }
3711 3759
3712 internal static bool QuaternionIsFinite(Quaternion q) 3760 internal static bool QuaternionIsFinite(Quaternion q)
3713 { 3761 {
3714 if (Single.IsNaN(q.X) || Single.IsInfinity(q.X)) 3762 if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
3715 return false; 3763 return false;
3716 if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y)) 3764 if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
3717 return false; 3765 return false;
3718 if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z)) 3766 if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
3719 return false; 3767 return false;
3720 if (Single.IsNaN(q.W) || Single.IsInfinity(q.W)) 3768 if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
3721 return false; 3769 return false;
3722 return true; 3770 return true;
3723 } 3771 }
3724 3772
3725 internal static void DMassCopy(ref d.Mass src, ref d.Mass dst) 3773 internal static void DMassCopy(ref d.Mass src, ref d.Mass dst)
3726 { 3774 {
3727 dst.c.W = src.c.W; 3775 dst.c.W = src.c.W;
3728 dst.c.X = src.c.X; 3776 dst.c.X = src.c.X;
3729 dst.c.Y = src.c.Y; 3777 dst.c.Y = src.c.Y;
3730 dst.c.Z = src.c.Z; 3778 dst.c.Z = src.c.Z;
3731 dst.mass = src.mass; 3779 dst.mass = src.mass;
3732 dst.I.M00 = src.I.M00; 3780 dst.I.M00 = src.I.M00;
3733 dst.I.M01 = src.I.M01; 3781 dst.I.M01 = src.I.M01;
3734 dst.I.M02 = src.I.M02; 3782 dst.I.M02 = src.I.M02;
3735 dst.I.M10 = src.I.M10; 3783 dst.I.M10 = src.I.M10;
3736 dst.I.M11 = src.I.M11; 3784 dst.I.M11 = src.I.M11;
3737 dst.I.M12 = src.I.M12; 3785 dst.I.M12 = src.I.M12;
3738 dst.I.M20 = src.I.M20; 3786 dst.I.M20 = src.I.M20;
3739 dst.I.M21 = src.I.M21; 3787 dst.I.M21 = src.I.M21;
3740 dst.I.M22 = src.I.M22; 3788 dst.I.M22 = src.I.M22;
3741 } 3789 }
3742 3790
3743 internal static void DMassSubPartFromObj(ref d.Mass part, ref d.Mass theobj) 3791 internal static void DMassSubPartFromObj(ref d.Mass part, ref d.Mass theobj)
3744 { 3792 {
3745 // assumes object center of mass is zero 3793 // assumes object center of mass is zero
3746 float smass = part.mass; 3794 float smass = part.mass;
3747 theobj.mass -= smass; 3795 theobj.mass -= smass;
3748 3796
3749 smass *= 1.0f / (theobj.mass); ; 3797 smass *= 1.0f / (theobj.mass); ;
3750 3798
3751 theobj.c.X -= part.c.X * smass; 3799 theobj.c.X -= part.c.X * smass;
3752 theobj.c.Y -= part.c.Y * smass; 3800 theobj.c.Y -= part.c.Y * smass;
3753 theobj.c.Z -= part.c.Z * smass; 3801 theobj.c.Z -= part.c.Z * smass;
3754 3802
3755 theobj.I.M00 -= part.I.M00; 3803 theobj.I.M00 -= part.I.M00;
3756 theobj.I.M01 -= part.I.M01; 3804 theobj.I.M01 -= part.I.M01;
3757 theobj.I.M02 -= part.I.M02; 3805 theobj.I.M02 -= part.I.M02;
3758 theobj.I.M10 -= part.I.M10; 3806 theobj.I.M10 -= part.I.M10;
3759 theobj.I.M11 -= part.I.M11; 3807 theobj.I.M11 -= part.I.M11;
3760 theobj.I.M12 -= part.I.M12; 3808 theobj.I.M12 -= part.I.M12;
3761 theobj.I.M20 -= part.I.M20; 3809 theobj.I.M20 -= part.I.M20;
3762 theobj.I.M21 -= part.I.M21; 3810 theobj.I.M21 -= part.I.M21;
3763 theobj.I.M22 -= part.I.M22; 3811 theobj.I.M22 -= part.I.M22;
3764 } 3812 }
3765 3813
3766 private static void DMassDup(ref d.Mass src, out d.Mass dst) 3814 private static void DMassDup(ref d.Mass src, out d.Mass dst)
3767 { 3815 {
3768 dst = new d.Mass { }; 3816 dst = new d.Mass { };
3769 3817
3770 dst.c.W = src.c.W; 3818 dst.c.W = src.c.W;
3771 dst.c.X = src.c.X; 3819 dst.c.X = src.c.X;
3772 dst.c.Y = src.c.Y; 3820 dst.c.Y = src.c.Y;
3773 dst.c.Z = src.c.Z; 3821 dst.c.Z = src.c.Z;
3774 dst.mass = src.mass; 3822 dst.mass = src.mass;
3775 dst.I.M00 = src.I.M00; 3823 dst.I.M00 = src.I.M00;
3776 dst.I.M01 = src.I.M01; 3824 dst.I.M01 = src.I.M01;
3777 dst.I.M02 = src.I.M02; 3825 dst.I.M02 = src.I.M02;
3778 dst.I.M10 = src.I.M10; 3826 dst.I.M10 = src.I.M10;
3779 dst.I.M11 = src.I.M11; 3827 dst.I.M11 = src.I.M11;
3780 dst.I.M12 = src.I.M12; 3828 dst.I.M12 = src.I.M12;
3781 dst.I.M20 = src.I.M20; 3829 dst.I.M20 = src.I.M20;
3782 dst.I.M21 = src.I.M21; 3830 dst.I.M21 = src.I.M21;
3783 dst.I.M22 = src.I.M22; 3831 dst.I.M22 = src.I.M22;
3784 } 3832 }
3785 private void donullchange() 3833 private void donullchange()
3786 { 3834 {
3787 } 3835 }
3788 3836
3789 public bool DoAChange(changes what, object arg) 3837 public bool DoAChange(changes what, object arg)
3790 { 3838 {
3791 if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.Remove) 3839 if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.Remove)
3792 { 3840 {
3793 return false; 3841 return false;
3794 } 3842 }
3795 3843
3796 // nasty switch 3844 // nasty switch
3797 switch (what) 3845 switch (what)
3798 { 3846 {
3799 case changes.Add: 3847 case changes.Add:
3800 changeadd(); 3848 changeadd();
3801 break; 3849 break;
3802 case changes.Remove: 3850 case changes.Remove:
3803 //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff... 3851 //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff...
3804 //When we return true, it destroys all of the prims in the linkset anyway 3852 //When we return true, it destroys all of the prims in the linkset anyway
3805 if (_parent != null) 3853 if (_parent != null)
3806 { 3854 {
3807 OdePrim parent = (OdePrim)_parent; 3855 OdePrim parent = (OdePrim)_parent;
3808 parent.ChildRemove(this, false); 3856 parent.ChildRemove(this, false);
3809 } 3857 }
3810 else 3858 else
3811 ChildRemove(this, false); 3859 ChildRemove(this, false);
3812 3860
3813 m_vehicle = null; 3861 m_vehicle = null;
3814 RemoveGeom(); 3862 RemoveGeom();
3815 m_targetSpace = IntPtr.Zero; 3863 m_targetSpace = IntPtr.Zero;
3816 if (m_eventsubscription > 0) 3864 if (m_eventsubscription > 0)
3817 UnSubscribeEvents(); 3865 UnSubscribeEvents();
3818 return true; 3866 return true;
3819 3867
3820 case changes.Link: 3868 case changes.Link:
3821 OdePrim tmp = (OdePrim)arg; 3869 OdePrim tmp = (OdePrim)arg;
3822 changeLink(tmp); 3870 changeLink(tmp);
3823 break; 3871 break;
3824 3872
3825 case changes.DeLink: 3873 case changes.DeLink:
3826 changeLink(null); 3874 changeLink(null);
3827 break; 3875 break;
3828 3876
3829 case changes.Position: 3877 case changes.Position:
3830 changePosition((Vector3)arg); 3878 changePosition((Vector3)arg);
3831 break; 3879 break;
3832 3880
3833 case changes.Orientation: 3881 case changes.Orientation:
3834 changeOrientation((Quaternion)arg); 3882 changeOrientation((Quaternion)arg);
3835 break; 3883 break;
3836 3884
3837 case changes.PosOffset: 3885 case changes.PosOffset:
3838 donullchange(); 3886 donullchange();
3839 break; 3887 break;
3840 3888
3841 case changes.OriOffset: 3889 case changes.OriOffset:
3842 donullchange(); 3890 donullchange();
3843 break; 3891 break;
3844 3892
3845 case changes.Velocity: 3893 case changes.Velocity:
3846 changevelocity((Vector3)arg); 3894 changevelocity((Vector3)arg);
3847 break; 3895 break;
3848 3896
3849 // case changes.Acceleration: 3897 // case changes.Acceleration:
3850 // changeacceleration((Vector3)arg); 3898 // changeacceleration((Vector3)arg);
3851 // break; 3899 // break;
3852 // case changes.AngVelocity: 3900 // case changes.AngVelocity:
3853 // changeangvelocity((Vector3)arg); 3901 // changeangvelocity((Vector3)arg);
3854 // break; 3902 // break;
3855 3903
3856 case changes.Force: 3904 case changes.Force:
3857 changeForce((Vector3)arg); 3905 changeForce((Vector3)arg);
3858 break; 3906 break;
3859 3907
3860 case changes.Torque: 3908 case changes.Torque:
3861 changeSetTorque((Vector3)arg); 3909 changeSetTorque((Vector3)arg);
3862 break; 3910 break;
3863 3911
3864 case changes.AddForce: 3912 case changes.AddForce:
3865 changeAddForce((Vector3)arg); 3913 changeAddForce((Vector3)arg);
3866 break; 3914 break;
3867 3915
3868 case changes.AddAngForce: 3916 case changes.AddAngForce:
3869 changeAddAngularForce((Vector3)arg); 3917 changeAddAngularForce((Vector3)arg);
3870 break; 3918 break;
3871 3919
3872 case changes.AngLock: 3920 case changes.AngLock:
3873 changeAngularLock((Vector3)arg); 3921 changeAngularLock((Vector3)arg);
3874 break; 3922 break;
3875 3923
3876 case changes.Size: 3924 case changes.Size:
3877 changeSize((Vector3)arg); 3925 changeSize((Vector3)arg);
3878 break; 3926 break;
3879 3927
3880 case changes.Shape: 3928 case changes.Shape:
3881 changeShape((PrimitiveBaseShape)arg); 3929 changeShape((PrimitiveBaseShape)arg);
3882 break; 3930 break;
3883 3931
3884 case changes.CollidesWater: 3932 case changes.CollidesWater:
3885 changeFloatOnWater((bool)arg); 3933 changeFloatOnWater((bool)arg);
3886 break; 3934 break;
3887 3935
3888 case changes.VolumeDtc: 3936 case changes.VolumeDtc:
3889 changeVolumedetetion((bool)arg); 3937 changeVolumedetetion((bool)arg);
3890 break; 3938 break;
3891 3939
3892 case changes.Phantom: 3940 case changes.Phantom:
3893 changePhantomStatus((bool)arg); 3941 changePhantomStatus((bool)arg);
3894 break; 3942 break;
3895 3943
3896 case changes.Physical: 3944 case changes.Physical:
3897 changePhysicsStatus((bool)arg); 3945 changePhysicsStatus((bool)arg);
3898 break; 3946 break;
3899 3947
3900 case changes.Selected: 3948 case changes.Selected:
3901 changeSelectedStatus((bool)arg); 3949 changeSelectedStatus((bool)arg);
3902 break; 3950 break;
3903 3951
3904 case changes.disabled: 3952 case changes.disabled:
3905 changeDisable((bool)arg); 3953 changeDisable((bool)arg);
3906 break; 3954 break;
3907 3955
3908 case changes.building: 3956 case changes.building:
3909 changeBuilding((bool)arg); 3957 changeBuilding((bool)arg);
3910 break; 3958 break;
3911 3959
3912 case changes.VehicleType: 3960 case changes.VehicleType:
3913 changeVehicleType((int)arg); 3961 changeVehicleType((int)arg);
3914 break; 3962 break;
3915 3963
3916 case changes.VehicleFlags: 3964 case changes.VehicleFlags:
3917 changeVehicleFlags((strVehicleBoolParam) arg); 3965 changeVehicleFlags((strVehicleBoolParam) arg);
3918 break; 3966 break;
3919 3967
3920 case changes.VehicleFloatParam: 3968 case changes.VehicleFloatParam:
3921 changeVehicleFloatParam((strVehicleFloatParam) arg); 3969 changeVehicleFloatParam((strVehicleFloatParam) arg);
3922 break; 3970 break;
3923 3971
3924 case changes.VehicleVectorParam: 3972 case changes.VehicleVectorParam:
3925 changeVehicleVectorParam((strVehicleVectorParam) arg); 3973 changeVehicleVectorParam((strVehicleVectorParam) arg);
3926 break; 3974 break;
3927 3975
3928 case changes.VehicleRotationParam: 3976 case changes.VehicleRotationParam:
3929 changeVehicleRotationParam((strVehicleQuatParam) arg); 3977 changeVehicleRotationParam((strVehicleQuatParam) arg);
3930 break; 3978 break;
3931 3979
3932 case changes.SetVehicle: 3980 case changes.SetVehicle:
3933 changeSetVehicle((VehicleData) arg); 3981 changeSetVehicle((VehicleData) arg);
3934 break; 3982 break;
3935 case changes.Null: 3983 case changes.Null:
3936 donullchange(); 3984 donullchange();
3937 break; 3985 break;
3938 3986
3939 default: 3987 default:
3940 donullchange(); 3988 donullchange();
3941 break; 3989 break;
3942 } 3990 }
3943 return false; 3991 return false;
3944 } 3992 }
3945 3993
3946 public void AddChange(changes what, object arg) 3994 public void AddChange(changes what, object arg)
3947 { 3995 {
3948 _parent_scene.AddChange((PhysicsActor) this, what, arg); 3996 _parent_scene.AddChange((PhysicsActor) this, what, arg);
3949 } 3997 }
3950 3998
3951 3999
3952 private struct strVehicleBoolParam 4000 private struct strVehicleBoolParam
3953 { 4001 {
3954 public int param; 4002 public int param;
3955 public bool value; 4003 public bool value;
3956 } 4004 }
3957 4005
3958 private struct strVehicleFloatParam 4006 private struct strVehicleFloatParam
3959 { 4007 {
3960 public int param; 4008 public int param;
3961 public float value; 4009 public float value;
3962 } 4010 }
3963 4011
3964 private struct strVehicleQuatParam 4012 private struct strVehicleQuatParam
3965 { 4013 {
3966 public int param; 4014 public int param;
3967 public Quaternion value; 4015 public Quaternion value;
3968 } 4016 }
3969 4017
3970 private struct strVehicleVectorParam 4018 private struct strVehicleVectorParam
3971 { 4019 {
3972 public int param; 4020 public int param;
3973 public Vector3 value; 4021 public Vector3 value;
3974 } 4022 }
3975 } 4023 }
3976} \ No newline at end of file 4024} \ No newline at end of file
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
index 3fc2de3..1a6907d 100644
--- a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
@@ -160,8 +160,8 @@ namespace OpenSim.Region.Physics.OdePlugin
160 private Random fluidRandomizer = new Random(Environment.TickCount); 160 private Random fluidRandomizer = new Random(Environment.TickCount);
161 161
162 const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce; 162 const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce;
163 const float comumContactERP = 0.6f; 163 const float MaxERP = 0.8f;
164 const float comumSoftContactERP = 0.1f; 164 const float minERP = 0.1f;
165 const float comumContactCFM = 0.0001f; 165 const float comumContactCFM = 0.0001f;
166 166
167 float frictionMovementMult = 0.3f; 167 float frictionMovementMult = 0.3f;
@@ -527,7 +527,7 @@ namespace OpenSim.Region.Physics.OdePlugin
527 527
528 // sets a global contact for a joint for contactgeom , and base contact description) 528 // sets a global contact for a joint for contactgeom , and base contact description)
529 529
530 private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom, float mu, float bounce, bool softerp) 530 private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom, float mu, float bounce,float cfm,float erp)
531 { 531 {
532 if (GlobalContactsArray == IntPtr.Zero || m_global_contactcount >= maxContactsbeforedeath) 532 if (GlobalContactsArray == IntPtr.Zero || m_global_contactcount >= maxContactsbeforedeath)
533 return IntPtr.Zero; 533 return IntPtr.Zero;
@@ -545,11 +545,8 @@ namespace OpenSim.Region.Physics.OdePlugin
545 newcontact.surface.mode = comumContactFlags; 545 newcontact.surface.mode = comumContactFlags;
546 newcontact.surface.mu = mu; 546 newcontact.surface.mu = mu;
547 newcontact.surface.bounce = bounce; 547 newcontact.surface.bounce = bounce;
548 newcontact.surface.soft_cfm = comumContactCFM; 548 newcontact.surface.soft_cfm = cfm;
549 if (softerp) 549 newcontact.surface.soft_erp = erp;
550 newcontact.surface.soft_erp = comumSoftContactERP;
551 else
552 newcontact.surface.soft_erp = comumContactERP;
553 550
554 IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf)); 551 IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf));
555 Marshal.StructureToPtr(newcontact, contact, true); 552 Marshal.StructureToPtr(newcontact, contact, true);
@@ -693,9 +690,11 @@ namespace OpenSim.Region.Physics.OdePlugin
693 // big messy collision analises 690 // big messy collision analises
694 float mu = 0; 691 float mu = 0;
695 float bounce = 0; 692 float bounce = 0;
693 float cfm = 0.0001f;
694 float erp = 0.1f;
695
696 ContactData contactdata1 = new ContactData(0, 0, false); 696 ContactData contactdata1 = new ContactData(0, 0, false);
697 ContactData contactdata2 = new ContactData(0, 0, false); 697 ContactData contactdata2 = new ContactData(0, 0, false);
698 bool erpSoft = false;
699 698
700 String name = null; 699 String name = null;
701 bool dop1foot = false; 700 bool dop1foot = false;
@@ -705,61 +704,65 @@ namespace OpenSim.Region.Physics.OdePlugin
705 switch (p1.PhysicsActorType) 704 switch (p1.PhysicsActorType)
706 { 705 {
707 case (int)ActorTypes.Agent: 706 case (int)ActorTypes.Agent:
708 switch (p2.PhysicsActorType)
709 { 707 {
710 case (int)ActorTypes.Agent: 708 bounce = 0;
711 p1.getContactData(ref contactdata1); 709 mu = 0;
712 p2.getContactData(ref contactdata2); 710 cfm = 0.0001f;
713 711
714 bounce = 0; 712 switch (p2.PhysicsActorType)
715 713 {
716 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); 714 case (int)ActorTypes.Agent:
715/*
716 p1.getContactData(ref contactdata1);
717 p2.getContactData(ref contactdata2);
717 718
718 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) 719 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
719 mu *= frictionMovementMult;
720 720
721 erpSoft = contactdata1.softcolide | contactdata2.softcolide; 721 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
722 p1.CollidingObj = true; 722 mu *= frictionMovementMult;
723 p2.CollidingObj = true; 723*/
724 break; 724 p1.CollidingObj = true;
725 case (int)ActorTypes.Prim: 725 p2.CollidingObj = true;
726 p1.getContactData(ref contactdata1); 726 break;
727 p2.getContactData(ref contactdata2); 727 case (int)ActorTypes.Prim:
728/*
729 p1.getContactData(ref contactdata1);
730 p2.getContactData(ref contactdata2);
728 731
729 bounce = 0;
730
731 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
732 732
733 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) 733 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
734 mu *= frictionMovementMult;
735 if (p2.Velocity.LengthSquared() > 0.0f)
736 p2.CollidingObj = true;
737 734
738 erpSoft = contactdata1.softcolide | contactdata2.softcolide; 735 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
736 mu *= frictionMovementMult;
737 */
738 if (p2.Velocity.LengthSquared() > 0.0f)
739 p2.CollidingObj = true;
739 740
740 dop1foot = true; 741 dop1foot = true;
741 break; 742 break;
742 default: 743 default:
743 ignore=true; // avatar to terrain and water ignored 744 ignore = true; // avatar to terrain and water ignored
744 break; 745 break;
746 }
747 break;
745 } 748 }
746 break;
747 749
748 case (int)ActorTypes.Prim: 750 case (int)ActorTypes.Prim:
749 switch (p2.PhysicsActorType) 751 switch (p2.PhysicsActorType)
750 { 752 {
751 case (int)ActorTypes.Agent: 753 case (int)ActorTypes.Agent:
752 p1.getContactData(ref contactdata1); 754// p1.getContactData(ref contactdata1);
753 p2.getContactData(ref contactdata2); 755// p2.getContactData(ref contactdata2);
754 756
755 bounce = 0; 757 bounce = 0;
756 758 mu = 0;
759 cfm = 0.0001f;
760/*
757 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); 761 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
758 762
759 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) 763 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
760 mu *= frictionMovementMult; 764 mu *= frictionMovementMult;
761 765*/
762 erpSoft = contactdata1.softcolide | contactdata2.softcolide;
763 dop2foot = true; 766 dop2foot = true;
764 if (p1.Velocity.LengthSquared() > 0.0f) 767 if (p1.Velocity.LengthSquared() > 0.0f)
765 p1.CollidingObj = true; 768 p1.CollidingObj = true;
@@ -773,9 +776,16 @@ namespace OpenSim.Region.Physics.OdePlugin
773 p1.getContactData(ref contactdata1); 776 p1.getContactData(ref contactdata1);
774 p2.getContactData(ref contactdata2); 777 p2.getContactData(ref contactdata2);
775 bounce = contactdata1.bounce * contactdata2.bounce; 778 bounce = contactdata1.bounce * contactdata2.bounce;
776 erpSoft = contactdata1.softcolide | contactdata2.softcolide;
777 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); 779 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
778 780
781 cfm = p1.Mass;
782 if (cfm > p2.Mass)
783 cfm = p2.Mass;
784 cfm = (float)Math.Sqrt(cfm);
785 cfm *= 0.0001f;
786 if (cfm > 0.8f)
787 cfm = 0.8f;
788
779 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f)) 789 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
780 mu *= frictionMovementMult; 790 mu *= frictionMovementMult;
781 791
@@ -790,12 +800,17 @@ namespace OpenSim.Region.Physics.OdePlugin
790 mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction); 800 mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction);
791 if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f) 801 if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f)
792 mu *= frictionMovementMult; 802 mu *= frictionMovementMult;
793 erpSoft = contactdata1.softcolide;
794 p1.CollidingGround = true; 803 p1.CollidingGround = true;
804 cfm = p1.Mass;
805 cfm = (float)Math.Sqrt(cfm);
806 cfm *= 0.0001f;
807 if (cfm > 0.8f)
808 cfm = 0.8f;
809
795 } 810 }
796 else if (name == "Water") 811 else if (name == "Water")
797 { 812 {
798 erpSoft = true; 813 ignore = true;
799 } 814 }
800 } 815 }
801 else 816 else
@@ -815,7 +830,11 @@ namespace OpenSim.Region.Physics.OdePlugin
815 p2.getContactData(ref contactdata2); 830 p2.getContactData(ref contactdata2);
816 bounce = contactdata2.bounce * TerrainBounce; 831 bounce = contactdata2.bounce * TerrainBounce;
817 mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction); 832 mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction);
818 erpSoft = contactdata2.softcolide; 833 cfm = p2.Mass;
834 cfm = (float)Math.Sqrt(cfm);
835 cfm *= 0.0001f;
836 if (cfm > 0.8f)
837 cfm = 0.8f;
819 838
820 if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f) 839 if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f)
821 mu *= frictionMovementMult; 840 mu *= frictionMovementMult;
@@ -827,7 +846,7 @@ namespace OpenSim.Region.Physics.OdePlugin
827 else if (name == "Water" && 846 else if (name == "Water" &&
828 (p2.PhysicsActorType == (int)ActorTypes.Prim || p2.PhysicsActorType == (int)ActorTypes.Agent)) 847 (p2.PhysicsActorType == (int)ActorTypes.Prim || p2.PhysicsActorType == (int)ActorTypes.Agent))
829 { 848 {
830 erpSoft = true; 849 ignore = true;
831 } 850 }
832 } 851 }
833 else 852 else
@@ -848,7 +867,14 @@ namespace OpenSim.Region.Physics.OdePlugin
848 if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f) 867 if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f)
849 p2.IsColliding = true; 868 p2.IsColliding = true;
850 869
851 Joint = CreateContacJoint(ref curContact, mu, bounce, erpSoft); 870
871 erp = curContact.depth;
872 if (erp < minERP)
873 erp = minERP;
874 else if (erp > MaxERP)
875 erp = MaxERP;
876
877 Joint = CreateContacJoint(ref curContact, mu, bounce,cfm,erp);
852 d.JointAttach(Joint, b1, b2); 878 d.JointAttach(Joint, b1, b2);
853 879
854 if (++m_global_contactcount >= maxContactsbeforedeath) 880 if (++m_global_contactcount >= maxContactsbeforedeath)