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