diff options
author | UbitUmarov | 2012-03-24 11:30:29 +0000 |
---|---|---|
committer | UbitUmarov | 2012-03-24 11:30:29 +0000 |
commit | 21a97408d4b209f22dabbe1203cffc388d9757bf (patch) | |
tree | 92de096526a68b03dedddf28d45e3dbd733645c5 /OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | |
parent | Merge branch 'master' of ssh://3dhosting.de/var/git/careminster into ubitwork (diff) | |
download | opensim-SC-21a97408d4b209f22dabbe1203cffc388d9757bf.zip opensim-SC-21a97408d4b209f22dabbe1203cffc388d9757bf.tar.gz opensim-SC-21a97408d4b209f22dabbe1203cffc388d9757bf.tar.bz2 opensim-SC-21a97408d4b209f22dabbe1203cffc388d9757bf.tar.xz |
Avatars have no bounce
Diffstat (limited to '')
-rw-r--r-- | OpenSim/Region/Physics/UbitOdePlugin/ODEPrim.cs | 7645 |
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 | ||
49 | using System; | 49 | using System; |
50 | using System.Collections.Generic; | 50 | using System.Collections.Generic; |
51 | using System.Reflection; | 51 | using System.Reflection; |
52 | using System.Runtime.InteropServices; | 52 | using System.Runtime.InteropServices; |
53 | using System.Threading; | 53 | using System.Threading; |
54 | using log4net; | 54 | using log4net; |
55 | using OpenMetaverse; | 55 | using OpenMetaverse; |
56 | using OdeAPI; | 56 | using OdeAPI; |
57 | using OpenSim.Framework; | 57 | using OpenSim.Framework; |
58 | using OpenSim.Region.Physics.Manager; | 58 | using OpenSim.Region.Physics.Manager; |
59 | 59 | ||
60 | 60 | ||
61 | namespace OpenSim.Region.Physics.OdePlugin | 61 | namespace 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 |