diff options
Diffstat (limited to 'OpenSim/Region')
29 files changed, 9286 insertions, 0 deletions
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs new file mode 100644 index 0000000..9f9ebcc --- /dev/null +++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs | |||
@@ -0,0 +1,434 @@ | |||
1 | /* | ||
2 | * Copyright (c) Contributors, http://opensimulator.org/ | ||
3 | * See CONTRIBUTORS.TXT for a full list of copyright holders. | ||
4 | * | ||
5 | * Redistribution and use in source and binary forms, with or without | ||
6 | * modification, are permitted provided that the following conditions are met: | ||
7 | * * Redistributions of source code must retain the above copyright | ||
8 | * notice, this list of conditions and the following disclaimer. | ||
9 | * * Redistributions in binary form must reproduce the above copyrightD | ||
10 | * notice, this list of conditions and the following disclaimer in the | ||
11 | * documentation and/or other materials provided with the distribution. | ||
12 | * * Neither the name of the OpenSimulator Project nor the | ||
13 | * names of its contributors may be used to endorse or promote products | ||
14 | * derived from this software without specific prior written permission. | ||
15 | * | ||
16 | * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY | ||
17 | * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED | ||
18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE | ||
19 | * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY | ||
20 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES | ||
21 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
22 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND | ||
23 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
24 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS | ||
25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. | ||
26 | */ | ||
27 | using System; | ||
28 | using System.Collections.Generic; | ||
29 | using System.Reflection; | ||
30 | using log4net; | ||
31 | using OpenMetaverse; | ||
32 | using OpenSim.Framework; | ||
33 | using OpenSim.Region.Physics.Manager; | ||
34 | |||
35 | namespace OpenSim.Region.Physics.BulletSPlugin | ||
36 | { | ||
37 | public class BSCharacter : PhysicsActor | ||
38 | { | ||
39 | private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); | ||
40 | private static readonly string LogHeader = "[BULLETS CHAR]"; | ||
41 | |||
42 | private BSScene _scene; | ||
43 | private String _avName; | ||
44 | private bool _stopped; | ||
45 | private Vector3 _size; | ||
46 | private Vector3 _scale; | ||
47 | private PrimitiveBaseShape _pbs; | ||
48 | private uint _localID = 0; | ||
49 | private bool _grabbed; | ||
50 | private bool _selected; | ||
51 | private Vector3 _position; | ||
52 | private float _mass = 80f; | ||
53 | public float _density = 60f; | ||
54 | private Vector3 _force; | ||
55 | private Vector3 _velocity; | ||
56 | private Vector3 _torque; | ||
57 | private float _collisionScore; | ||
58 | private Vector3 _acceleration; | ||
59 | private Quaternion _orientation; | ||
60 | private int _physicsActorType; | ||
61 | private bool _isPhysical; | ||
62 | private bool _flying; | ||
63 | private bool _setAlwaysRun; | ||
64 | private bool _throttleUpdates; | ||
65 | private bool _isColliding; | ||
66 | private long _collidingStep; | ||
67 | private bool _collidingGround; | ||
68 | private long _collidingGroundStep; | ||
69 | private bool _collidingObj; | ||
70 | private bool _floatOnWater; | ||
71 | private Vector3 _rotationalVelocity; | ||
72 | private bool _kinematic; | ||
73 | private float _buoyancy; | ||
74 | |||
75 | private int _subscribedEventsMs = 0; | ||
76 | private int _lastCollisionTime = 0; | ||
77 | |||
78 | private Vector3 _PIDTarget; | ||
79 | private bool _usePID; | ||
80 | private float _PIDTau; | ||
81 | private bool _useHoverPID; | ||
82 | private float _PIDHoverHeight; | ||
83 | private PIDHoverType _PIDHoverType; | ||
84 | private float _PIDHoverTao; | ||
85 | |||
86 | public BSCharacter(uint localID, String avName, BSScene parent_scene, Vector3 pos, Vector3 size, bool isFlying) | ||
87 | { | ||
88 | _localID = localID; | ||
89 | _avName = avName; | ||
90 | _scene = parent_scene; | ||
91 | _position = pos; | ||
92 | _size = size; | ||
93 | _orientation = Quaternion.Identity; | ||
94 | _velocity = Vector3.Zero; | ||
95 | _buoyancy = 0f; // characters return a buoyancy of zero | ||
96 | _scale = new Vector3(1f, 1f, 1f); | ||
97 | float AVvolume = (float) (Math.PI*Math.Pow(_scene.Params.avatarCapsuleRadius, 2)*_scene.Params.avatarCapsuleHeight); | ||
98 | _density = _scene.Params.avatarDensity; | ||
99 | _mass = _density*AVvolume; | ||
100 | |||
101 | ShapeData shapeData = new ShapeData(); | ||
102 | shapeData.ID = _localID; | ||
103 | shapeData.Type = ShapeData.PhysicsShapeType.SHAPE_AVATAR; | ||
104 | shapeData.Position = _position; | ||
105 | shapeData.Rotation = _orientation; | ||
106 | shapeData.Velocity = _velocity; | ||
107 | shapeData.Scale = _scale; | ||
108 | shapeData.Mass = _mass; | ||
109 | shapeData.Buoyancy = isFlying ? 1f : 0f; | ||
110 | shapeData.Static = ShapeData.numericFalse; | ||
111 | shapeData.Friction = _scene.Params.avatarFriction; | ||
112 | shapeData.Restitution = _scene.Params.defaultRestitution; | ||
113 | |||
114 | // do actual create at taint time | ||
115 | _scene.TaintedObject(delegate() | ||
116 | { | ||
117 | BulletSimAPI.CreateObject(parent_scene.WorldID, shapeData); | ||
118 | }); | ||
119 | |||
120 | return; | ||
121 | } | ||
122 | |||
123 | // called when this character is being destroyed and the resources should be released | ||
124 | public void Destroy() | ||
125 | { | ||
126 | _scene.TaintedObject(delegate() | ||
127 | { | ||
128 | BulletSimAPI.DestroyObject(_scene.WorldID, _localID); | ||
129 | }); | ||
130 | } | ||
131 | |||
132 | public override void RequestPhysicsterseUpdate() | ||
133 | { | ||
134 | base.RequestPhysicsterseUpdate(); | ||
135 | } | ||
136 | |||
137 | public override bool Stopped { | ||
138 | get { return _stopped; } | ||
139 | } | ||
140 | public override Vector3 Size { | ||
141 | get { return _size; } | ||
142 | set { _size = value; | ||
143 | } | ||
144 | } | ||
145 | public override PrimitiveBaseShape Shape { | ||
146 | set { _pbs = value; | ||
147 | } | ||
148 | } | ||
149 | public override uint LocalID { | ||
150 | set { _localID = value; | ||
151 | } | ||
152 | get { return _localID; } | ||
153 | } | ||
154 | public override bool Grabbed { | ||
155 | set { _grabbed = value; | ||
156 | } | ||
157 | } | ||
158 | public override bool Selected { | ||
159 | set { _selected = value; | ||
160 | } | ||
161 | } | ||
162 | public override void CrossingFailure() { return; } | ||
163 | public override void link(PhysicsActor obj) { return; } | ||
164 | public override void delink() { return; } | ||
165 | public override void LockAngularMotion(Vector3 axis) { return; } | ||
166 | |||
167 | public override Vector3 Position { | ||
168 | get { | ||
169 | // _position = BulletSimAPI.GetObjectPosition(_scene.WorldID, _localID); | ||
170 | return _position; | ||
171 | } | ||
172 | set { | ||
173 | _position = value; | ||
174 | _scene.TaintedObject(delegate() | ||
175 | { | ||
176 | BulletSimAPI.SetObjectTranslation(_scene.WorldID, _localID, _position, _orientation); | ||
177 | }); | ||
178 | } | ||
179 | } | ||
180 | public override float Mass { | ||
181 | get { | ||
182 | return _mass; | ||
183 | } | ||
184 | } | ||
185 | public override Vector3 Force { | ||
186 | get { return _force; } | ||
187 | set { | ||
188 | _force = value; | ||
189 | // m_log.DebugFormat("{0}: Force = {1}", LogHeader, _force); | ||
190 | _scene.TaintedObject(delegate() | ||
191 | { | ||
192 | BulletSimAPI.SetObjectForce(_scene.WorldID, _localID, _force); | ||
193 | }); | ||
194 | } | ||
195 | } | ||
196 | |||
197 | public override int VehicleType { | ||
198 | get { return 0; } | ||
199 | set { return; } | ||
200 | } | ||
201 | public override void VehicleFloatParam(int param, float value) { } | ||
202 | public override void VehicleVectorParam(int param, Vector3 value) {} | ||
203 | public override void VehicleRotationParam(int param, Quaternion rotation) { } | ||
204 | public override void VehicleFlags(int param, bool remove) { } | ||
205 | |||
206 | // Allows the detection of collisions with inherently non-physical prims. see llVolumeDetect for more | ||
207 | public override void SetVolumeDetect(int param) { return; } | ||
208 | |||
209 | public override Vector3 GeometricCenter { get { return Vector3.Zero; } } | ||
210 | public override Vector3 CenterOfMass { get { return Vector3.Zero; } } | ||
211 | public override Vector3 Velocity { | ||
212 | get { return _velocity; } | ||
213 | set { | ||
214 | _velocity = value; | ||
215 | _scene.TaintedObject(delegate() | ||
216 | { | ||
217 | BulletSimAPI.SetObjectVelocity(_scene.WorldID, _localID, _velocity); | ||
218 | }); | ||
219 | } | ||
220 | } | ||
221 | public override Vector3 Torque { | ||
222 | get { return _torque; } | ||
223 | set { _torque = value; | ||
224 | } | ||
225 | } | ||
226 | public override float CollisionScore { | ||
227 | get { return _collisionScore; } | ||
228 | set { _collisionScore = value; | ||
229 | } | ||
230 | } | ||
231 | public override Vector3 Acceleration { | ||
232 | get { return _acceleration; } | ||
233 | } | ||
234 | public override Quaternion Orientation { | ||
235 | get { return _orientation; } | ||
236 | set { | ||
237 | _orientation = value; | ||
238 | _scene.TaintedObject(delegate() | ||
239 | { | ||
240 | // _position = BulletSimAPI.GetObjectPosition(_scene.WorldID, _localID); | ||
241 | BulletSimAPI.SetObjectTranslation(_scene.WorldID, _localID, _position, _orientation); | ||
242 | }); | ||
243 | } | ||
244 | } | ||
245 | public override int PhysicsActorType { | ||
246 | get { return _physicsActorType; } | ||
247 | set { _physicsActorType = value; | ||
248 | } | ||
249 | } | ||
250 | public override bool IsPhysical { | ||
251 | get { return _isPhysical; } | ||
252 | set { _isPhysical = value; | ||
253 | } | ||
254 | } | ||
255 | public override bool Flying { | ||
256 | get { return _flying; } | ||
257 | set { | ||
258 | _flying = value; | ||
259 | _scene.TaintedObject(delegate() | ||
260 | { | ||
261 | // simulate flying by changing the effect of gravity | ||
262 | BulletSimAPI.SetObjectBuoyancy(_scene.WorldID, LocalID, _flying ? 1f : 0f); | ||
263 | }); | ||
264 | } | ||
265 | } | ||
266 | public override bool | ||
267 | SetAlwaysRun { | ||
268 | get { return _setAlwaysRun; } | ||
269 | set { _setAlwaysRun = value; } | ||
270 | } | ||
271 | public override bool ThrottleUpdates { | ||
272 | get { return _throttleUpdates; } | ||
273 | set { _throttleUpdates = value; } | ||
274 | } | ||
275 | public override bool IsColliding { | ||
276 | get { return (_collidingStep == _scene.SimulationStep); } | ||
277 | set { _isColliding = value; } | ||
278 | } | ||
279 | public override bool CollidingGround { | ||
280 | get { return (_collidingGroundStep == _scene.SimulationStep); } | ||
281 | set { _collidingGround = value; } | ||
282 | } | ||
283 | public override bool CollidingObj { | ||
284 | get { return _collidingObj; } | ||
285 | set { _collidingObj = value; } | ||
286 | } | ||
287 | public override bool FloatOnWater { | ||
288 | set { _floatOnWater = value; } | ||
289 | } | ||
290 | public override Vector3 RotationalVelocity { | ||
291 | get { return _rotationalVelocity; } | ||
292 | set { _rotationalVelocity = value; } | ||
293 | } | ||
294 | public override bool Kinematic { | ||
295 | get { return _kinematic; } | ||
296 | set { _kinematic = value; } | ||
297 | } | ||
298 | public override float Buoyancy { | ||
299 | get { return _buoyancy; } | ||
300 | set { _buoyancy = value; | ||
301 | _scene.TaintedObject(delegate() | ||
302 | { | ||
303 | // simulate flying by changing the effect of gravity | ||
304 | BulletSimAPI.SetObjectBuoyancy(_scene.WorldID, LocalID, _buoyancy); | ||
305 | }); | ||
306 | } | ||
307 | } | ||
308 | |||
309 | // Used for MoveTo | ||
310 | public override Vector3 PIDTarget { | ||
311 | set { _PIDTarget = value; } | ||
312 | } | ||
313 | public override bool PIDActive { | ||
314 | set { _usePID = value; } | ||
315 | } | ||
316 | public override float PIDTau { | ||
317 | set { _PIDTau = value; } | ||
318 | } | ||
319 | |||
320 | // Used for llSetHoverHeight and maybe vehicle height | ||
321 | // Hover Height will override MoveTo target's Z | ||
322 | public override bool PIDHoverActive { | ||
323 | set { _useHoverPID = value; } | ||
324 | } | ||
325 | public override float PIDHoverHeight { | ||
326 | set { _PIDHoverHeight = value; } | ||
327 | } | ||
328 | public override PIDHoverType PIDHoverType { | ||
329 | set { _PIDHoverType = value; } | ||
330 | } | ||
331 | public override float PIDHoverTau { | ||
332 | set { _PIDHoverTao = value; } | ||
333 | } | ||
334 | |||
335 | // For RotLookAt | ||
336 | public override Quaternion APIDTarget { set { return; } } | ||
337 | public override bool APIDActive { set { return; } } | ||
338 | public override float APIDStrength { set { return; } } | ||
339 | public override float APIDDamping { set { return; } } | ||
340 | |||
341 | public override void AddForce(Vector3 force, bool pushforce) { | ||
342 | if (force.IsFinite()) | ||
343 | { | ||
344 | _force.X += force.X; | ||
345 | _force.Y += force.Y; | ||
346 | _force.Z += force.Z; | ||
347 | _scene.TaintedObject(delegate() | ||
348 | { | ||
349 | BulletSimAPI.SetObjectForce(_scene.WorldID, _localID, _force); | ||
350 | }); | ||
351 | } | ||
352 | else | ||
353 | { | ||
354 | m_log.WarnFormat("{0}: Got a NaN force applied to a Character", LogHeader); | ||
355 | } | ||
356 | //m_lastUpdateSent = false; | ||
357 | } | ||
358 | public override void AddAngularForce(Vector3 force, bool pushforce) { | ||
359 | } | ||
360 | public override void SetMomentum(Vector3 momentum) { | ||
361 | } | ||
362 | public override void SubscribeEvents(int ms) { | ||
363 | _subscribedEventsMs = ms; | ||
364 | _lastCollisionTime = Util.EnvironmentTickCount() - _subscribedEventsMs; // make first collision happen | ||
365 | } | ||
366 | public override void UnSubscribeEvents() { | ||
367 | _subscribedEventsMs = 0; | ||
368 | } | ||
369 | public override bool SubscribedEvents() { | ||
370 | return (_subscribedEventsMs > 0); | ||
371 | } | ||
372 | |||
373 | // The physics engine says that properties have updated. Update same and inform | ||
374 | // the world that things have changed. | ||
375 | public void UpdateProperties(EntityProperties entprop) | ||
376 | { | ||
377 | bool changed = false; | ||
378 | // we assign to the local variables so the normal set action does not happen | ||
379 | if (_position != entprop.Position) | ||
380 | { | ||
381 | _position = entprop.Position; | ||
382 | changed = true; | ||
383 | } | ||
384 | if (_orientation != entprop.Rotation) | ||
385 | { | ||
386 | _orientation = entprop.Rotation; | ||
387 | changed = true; | ||
388 | } | ||
389 | if (_velocity != entprop.Velocity) | ||
390 | { | ||
391 | _velocity = entprop.Velocity; | ||
392 | changed = true; | ||
393 | } | ||
394 | if (_acceleration != entprop.Acceleration) | ||
395 | { | ||
396 | _acceleration = entprop.Acceleration; | ||
397 | changed = true; | ||
398 | } | ||
399 | if (_rotationalVelocity != entprop.RotationalVelocity) | ||
400 | { | ||
401 | _rotationalVelocity = entprop.RotationalVelocity; | ||
402 | changed = true; | ||
403 | } | ||
404 | if (changed) | ||
405 | { | ||
406 | // base.RequestPhysicsterseUpdate(); | ||
407 | } | ||
408 | } | ||
409 | |||
410 | public void Collide(uint collidingWith, ActorTypes type, Vector3 contactPoint, Vector3 contactNormal, float pentrationDepth) | ||
411 | { | ||
412 | // m_log.DebugFormat("{0}: Collide: ms={1}, id={2}, with={3}", LogHeader, _subscribedEventsMs, LocalID, collidingWith); | ||
413 | |||
414 | // The following makes IsColliding() and IsCollidingGround() work | ||
415 | _collidingStep = _scene.SimulationStep; | ||
416 | if (collidingWith == BSScene.TERRAIN_ID || collidingWith == BSScene.GROUNDPLANE_ID) | ||
417 | { | ||
418 | _collidingGroundStep = _scene.SimulationStep; | ||
419 | } | ||
420 | |||
421 | // throttle collisions to the rate specified in the subscription | ||
422 | if (_subscribedEventsMs == 0) return; // don't want collisions | ||
423 | int nowTime = _scene.SimulationNowTime; | ||
424 | if (nowTime < (_lastCollisionTime + _subscribedEventsMs)) return; | ||
425 | _lastCollisionTime = nowTime; | ||
426 | |||
427 | Dictionary<uint, ContactPoint> contactPoints = new Dictionary<uint, ContactPoint>(); | ||
428 | contactPoints.Add(collidingWith, new ContactPoint(contactPoint, contactNormal, pentrationDepth)); | ||
429 | CollisionEventUpdate args = new CollisionEventUpdate(LocalID, (int)type, 1, contactPoints); | ||
430 | base.SendCollisionUpdate(args); | ||
431 | } | ||
432 | |||
433 | } | ||
434 | } | ||
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs new file mode 100644 index 0000000..046726d --- /dev/null +++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs | |||
@@ -0,0 +1,951 @@ | |||
1 | /* | ||
2 | * Copyright (c) Contributors, http://opensimulator.org/ | ||
3 | * See CONTRIBUTORS.TXT for a full list of copyright holders. | ||
4 | * | ||
5 | * Redistribution and use in source and binary forms, with or without | ||
6 | * modification, are permitted provided that the following conditions are met: | ||
7 | * * Redistributions of source code must retain the above copyright | ||
8 | * notice, this list of conditions and the following disclaimer. | ||
9 | * * Redistributions in binary form must reproduce the above copyright | ||
10 | * notice, this list of conditions and the following disclaimer in the | ||
11 | * documentation and/or other materials provided with the distribution. | ||
12 | * * Neither the name of the OpenSimulator Project nor the | ||
13 | * names of its contributors may be used to endorse or promote products | ||
14 | * derived from this software without specific prior written permission. | ||
15 | * | ||
16 | * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY | ||
17 | * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED | ||
18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE | ||
19 | * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY | ||
20 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES | ||
21 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
22 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND | ||
23 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
24 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS | ||
25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. | ||
26 | */ | ||
27 | |||
28 | /* RA: June 14, 2011. Copied from ODEDynamics.cs and converted to | ||
29 | * call the BulletSim system. | ||
30 | */ | ||
31 | /* Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces | ||
32 | * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised: | ||
33 | * ODEPrim.cs contains methods dealing with Prim editing, Prim | ||
34 | * characteristics and Kinetic motion. | ||
35 | * ODEDynamics.cs contains methods dealing with Prim Physical motion | ||
36 | * (dynamics) and the associated settings. Old Linear and angular | ||
37 | * motors for dynamic motion have been replace with MoveLinear() | ||
38 | * and MoveAngular(); 'Physical' is used only to switch ODE dynamic | ||
39 | * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to | ||
40 | * switch between 'VEHICLE' parameter use and general dynamics | ||
41 | * settings use. | ||
42 | */ | ||
43 | |||
44 | using System; | ||
45 | using System.Collections.Generic; | ||
46 | using System.Reflection; | ||
47 | using System.Runtime.InteropServices; | ||
48 | using log4net; | ||
49 | using OpenMetaverse; | ||
50 | using OpenSim.Framework; | ||
51 | using OpenSim.Region.Physics.Manager; | ||
52 | |||
53 | namespace OpenSim.Region.Physics.BulletSPlugin | ||
54 | { | ||
55 | public class BSDynamics | ||
56 | { | ||
57 | private int frcount = 0; // Used to limit dynamics debug output to | ||
58 | // every 100th frame | ||
59 | |||
60 | // private BSScene m_parentScene = null; | ||
61 | private BSPrim m_prim; // the prim this dynamic controller belongs to | ||
62 | |||
63 | // Vehicle properties | ||
64 | private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind | ||
65 | public Vehicle Type | ||
66 | { | ||
67 | get { return m_type; } | ||
68 | } | ||
69 | // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier | ||
70 | private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings: | ||
71 | // HOVER_TERRAIN_ONLY | ||
72 | // HOVER_GLOBAL_HEIGHT | ||
73 | // NO_DEFLECTION_UP | ||
74 | // HOVER_WATER_ONLY | ||
75 | // HOVER_UP_ONLY | ||
76 | // LIMIT_MOTOR_UP | ||
77 | // LIMIT_ROLL_ONLY | ||
78 | private VehicleFlag m_Hoverflags = (VehicleFlag)0; | ||
79 | private Vector3 m_BlockingEndPoint = Vector3.Zero; | ||
80 | private Quaternion m_RollreferenceFrame = Quaternion.Identity; | ||
81 | // Linear properties | ||
82 | private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time | ||
83 | private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL | ||
84 | private Vector3 m_dir = Vector3.Zero; // velocity applied to body | ||
85 | private Vector3 m_linearFrictionTimescale = Vector3.Zero; | ||
86 | private float m_linearMotorDecayTimescale = 0; | ||
87 | private float m_linearMotorTimescale = 0; | ||
88 | private Vector3 m_lastLinearVelocityVector = Vector3.Zero; | ||
89 | private Vector3 m_lastPositionVector = Vector3.Zero; | ||
90 | // private bool m_LinearMotorSetLastFrame = false; | ||
91 | // private Vector3 m_linearMotorOffset = Vector3.Zero; | ||
92 | |||
93 | //Angular properties | ||
94 | private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor | ||
95 | private int m_angularMotorApply = 0; // application frame counter | ||
96 | private Vector3 m_angularMotorVelocity = Vector3.Zero; // current angular motor velocity | ||
97 | private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate | ||
98 | private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate | ||
99 | private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate | ||
100 | private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body | ||
101 | // private Vector3 m_lastVertAttractor = Vector3.Zero; // what VA was last applied to body | ||
102 | |||
103 | //Deflection properties | ||
104 | // private float m_angularDeflectionEfficiency = 0; | ||
105 | // private float m_angularDeflectionTimescale = 0; | ||
106 | // private float m_linearDeflectionEfficiency = 0; | ||
107 | // private float m_linearDeflectionTimescale = 0; | ||
108 | |||
109 | //Banking properties | ||
110 | // private float m_bankingEfficiency = 0; | ||
111 | // private float m_bankingMix = 0; | ||
112 | // private float m_bankingTimescale = 0; | ||
113 | |||
114 | //Hover and Buoyancy properties | ||
115 | private float m_VhoverHeight = 0f; | ||
116 | // private float m_VhoverEfficiency = 0f; | ||
117 | private float m_VhoverTimescale = 0f; | ||
118 | private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height | ||
119 | private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle. | ||
120 | // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity) | ||
121 | // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity. | ||
122 | // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity. | ||
123 | |||
124 | //Attractor properties | ||
125 | private float m_verticalAttractionEfficiency = 1.0f; // damped | ||
126 | private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor. | ||
127 | |||
128 | public BSDynamics(BSPrim myPrim) | ||
129 | { | ||
130 | m_prim = myPrim; | ||
131 | m_type = Vehicle.TYPE_NONE; | ||
132 | } | ||
133 | |||
134 | internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue) | ||
135 | { | ||
136 | switch (pParam) | ||
137 | { | ||
138 | case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY: | ||
139 | if (pValue < 0.01f) pValue = 0.01f; | ||
140 | // m_angularDeflectionEfficiency = pValue; | ||
141 | break; | ||
142 | case Vehicle.ANGULAR_DEFLECTION_TIMESCALE: | ||
143 | if (pValue < 0.01f) pValue = 0.01f; | ||
144 | // m_angularDeflectionTimescale = pValue; | ||
145 | break; | ||
146 | case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE: | ||
147 | if (pValue < 0.01f) pValue = 0.01f; | ||
148 | m_angularMotorDecayTimescale = pValue; | ||
149 | break; | ||
150 | case Vehicle.ANGULAR_MOTOR_TIMESCALE: | ||
151 | if (pValue < 0.01f) pValue = 0.01f; | ||
152 | m_angularMotorTimescale = pValue; | ||
153 | break; | ||
154 | case Vehicle.BANKING_EFFICIENCY: | ||
155 | if (pValue < 0.01f) pValue = 0.01f; | ||
156 | // m_bankingEfficiency = pValue; | ||
157 | break; | ||
158 | case Vehicle.BANKING_MIX: | ||
159 | if (pValue < 0.01f) pValue = 0.01f; | ||
160 | // m_bankingMix = pValue; | ||
161 | break; | ||
162 | case Vehicle.BANKING_TIMESCALE: | ||
163 | if (pValue < 0.01f) pValue = 0.01f; | ||
164 | // m_bankingTimescale = pValue; | ||
165 | break; | ||
166 | case Vehicle.BUOYANCY: | ||
167 | if (pValue < -1f) pValue = -1f; | ||
168 | if (pValue > 1f) pValue = 1f; | ||
169 | m_VehicleBuoyancy = pValue; | ||
170 | break; | ||
171 | // case Vehicle.HOVER_EFFICIENCY: | ||
172 | // if (pValue < 0f) pValue = 0f; | ||
173 | // if (pValue > 1f) pValue = 1f; | ||
174 | // m_VhoverEfficiency = pValue; | ||
175 | // break; | ||
176 | case Vehicle.HOVER_HEIGHT: | ||
177 | m_VhoverHeight = pValue; | ||
178 | break; | ||
179 | case Vehicle.HOVER_TIMESCALE: | ||
180 | if (pValue < 0.01f) pValue = 0.01f; | ||
181 | m_VhoverTimescale = pValue; | ||
182 | break; | ||
183 | case Vehicle.LINEAR_DEFLECTION_EFFICIENCY: | ||
184 | if (pValue < 0.01f) pValue = 0.01f; | ||
185 | // m_linearDeflectionEfficiency = pValue; | ||
186 | break; | ||
187 | case Vehicle.LINEAR_DEFLECTION_TIMESCALE: | ||
188 | if (pValue < 0.01f) pValue = 0.01f; | ||
189 | // m_linearDeflectionTimescale = pValue; | ||
190 | break; | ||
191 | case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE: | ||
192 | if (pValue < 0.01f) pValue = 0.01f; | ||
193 | m_linearMotorDecayTimescale = pValue; | ||
194 | break; | ||
195 | case Vehicle.LINEAR_MOTOR_TIMESCALE: | ||
196 | if (pValue < 0.01f) pValue = 0.01f; | ||
197 | m_linearMotorTimescale = pValue; | ||
198 | break; | ||
199 | case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY: | ||
200 | if (pValue < 0.1f) pValue = 0.1f; // Less goes unstable | ||
201 | if (pValue > 1.0f) pValue = 1.0f; | ||
202 | m_verticalAttractionEfficiency = pValue; | ||
203 | break; | ||
204 | case Vehicle.VERTICAL_ATTRACTION_TIMESCALE: | ||
205 | if (pValue < 0.01f) pValue = 0.01f; | ||
206 | m_verticalAttractionTimescale = pValue; | ||
207 | break; | ||
208 | |||
209 | // These are vector properties but the engine lets you use a single float value to | ||
210 | // set all of the components to the same value | ||
211 | case Vehicle.ANGULAR_FRICTION_TIMESCALE: | ||
212 | m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue); | ||
213 | break; | ||
214 | case Vehicle.ANGULAR_MOTOR_DIRECTION: | ||
215 | m_angularMotorDirection = new Vector3(pValue, pValue, pValue); | ||
216 | m_angularMotorApply = 10; | ||
217 | break; | ||
218 | case Vehicle.LINEAR_FRICTION_TIMESCALE: | ||
219 | m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue); | ||
220 | break; | ||
221 | case Vehicle.LINEAR_MOTOR_DIRECTION: | ||
222 | m_linearMotorDirection = new Vector3(pValue, pValue, pValue); | ||
223 | m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue); | ||
224 | break; | ||
225 | case Vehicle.LINEAR_MOTOR_OFFSET: | ||
226 | // m_linearMotorOffset = new Vector3(pValue, pValue, pValue); | ||
227 | break; | ||
228 | |||
229 | } | ||
230 | }//end ProcessFloatVehicleParam | ||
231 | |||
232 | internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue) | ||
233 | { | ||
234 | switch (pParam) | ||
235 | { | ||
236 | case Vehicle.ANGULAR_FRICTION_TIMESCALE: | ||
237 | m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
238 | break; | ||
239 | case Vehicle.ANGULAR_MOTOR_DIRECTION: | ||
240 | m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
241 | // Limit requested angular speed to 2 rps= 4 pi rads/sec | ||
242 | if (m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f; | ||
243 | if (m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f; | ||
244 | if (m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f; | ||
245 | if (m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f; | ||
246 | if (m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f; | ||
247 | if (m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f; | ||
248 | m_angularMotorApply = 10; | ||
249 | break; | ||
250 | case Vehicle.LINEAR_FRICTION_TIMESCALE: | ||
251 | m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
252 | break; | ||
253 | case Vehicle.LINEAR_MOTOR_DIRECTION: | ||
254 | m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
255 | m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
256 | break; | ||
257 | case Vehicle.LINEAR_MOTOR_OFFSET: | ||
258 | // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
259 | break; | ||
260 | case Vehicle.BLOCK_EXIT: | ||
261 | m_BlockingEndPoint = new Vector3(pValue.X, pValue.Y, pValue.Z); | ||
262 | break; | ||
263 | } | ||
264 | }//end ProcessVectorVehicleParam | ||
265 | |||
266 | internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue) | ||
267 | { | ||
268 | switch (pParam) | ||
269 | { | ||
270 | case Vehicle.REFERENCE_FRAME: | ||
271 | // m_referenceFrame = pValue; | ||
272 | break; | ||
273 | case Vehicle.ROLL_FRAME: | ||
274 | m_RollreferenceFrame = pValue; | ||
275 | break; | ||
276 | } | ||
277 | }//end ProcessRotationVehicleParam | ||
278 | |||
279 | internal void ProcessVehicleFlags(int pParam, bool remove) | ||
280 | { | ||
281 | if (remove) | ||
282 | { | ||
283 | if (pParam == -1) | ||
284 | { | ||
285 | m_flags = (VehicleFlag)0; | ||
286 | m_Hoverflags = (VehicleFlag)0; | ||
287 | return; | ||
288 | } | ||
289 | if ((pParam & (int)VehicleFlag.HOVER_GLOBAL_HEIGHT) == (int)VehicleFlag.HOVER_GLOBAL_HEIGHT) | ||
290 | { | ||
291 | if ((m_Hoverflags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != (VehicleFlag)0) | ||
292 | m_Hoverflags &= ~(VehicleFlag.HOVER_GLOBAL_HEIGHT); | ||
293 | } | ||
294 | if ((pParam & (int)VehicleFlag.HOVER_TERRAIN_ONLY) == (int)VehicleFlag.HOVER_TERRAIN_ONLY) | ||
295 | { | ||
296 | if ((m_Hoverflags & VehicleFlag.HOVER_TERRAIN_ONLY) != (VehicleFlag)0) | ||
297 | m_Hoverflags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY); | ||
298 | } | ||
299 | if ((pParam & (int)VehicleFlag.HOVER_UP_ONLY) == (int)VehicleFlag.HOVER_UP_ONLY) | ||
300 | { | ||
301 | if ((m_Hoverflags & VehicleFlag.HOVER_UP_ONLY) != (VehicleFlag)0) | ||
302 | m_Hoverflags &= ~(VehicleFlag.HOVER_UP_ONLY); | ||
303 | } | ||
304 | if ((pParam & (int)VehicleFlag.HOVER_WATER_ONLY) == (int)VehicleFlag.HOVER_WATER_ONLY) | ||
305 | { | ||
306 | if ((m_Hoverflags & VehicleFlag.HOVER_WATER_ONLY) != (VehicleFlag)0) | ||
307 | m_Hoverflags &= ~(VehicleFlag.HOVER_WATER_ONLY); | ||
308 | } | ||
309 | if ((pParam & (int)VehicleFlag.LIMIT_MOTOR_UP) == (int)VehicleFlag.LIMIT_MOTOR_UP) | ||
310 | { | ||
311 | if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != (VehicleFlag)0) | ||
312 | m_flags &= ~(VehicleFlag.LIMIT_MOTOR_UP); | ||
313 | } | ||
314 | if ((pParam & (int)VehicleFlag.LIMIT_ROLL_ONLY) == (int)VehicleFlag.LIMIT_ROLL_ONLY) | ||
315 | { | ||
316 | if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) != (VehicleFlag)0) | ||
317 | m_flags &= ~(VehicleFlag.LIMIT_ROLL_ONLY); | ||
318 | } | ||
319 | if ((pParam & (int)VehicleFlag.MOUSELOOK_BANK) == (int)VehicleFlag.MOUSELOOK_BANK) | ||
320 | { | ||
321 | if ((m_flags & VehicleFlag.MOUSELOOK_BANK) != (VehicleFlag)0) | ||
322 | m_flags &= ~(VehicleFlag.MOUSELOOK_BANK); | ||
323 | } | ||
324 | if ((pParam & (int)VehicleFlag.MOUSELOOK_STEER) == (int)VehicleFlag.MOUSELOOK_STEER) | ||
325 | { | ||
326 | if ((m_flags & VehicleFlag.MOUSELOOK_STEER) != (VehicleFlag)0) | ||
327 | m_flags &= ~(VehicleFlag.MOUSELOOK_STEER); | ||
328 | } | ||
329 | if ((pParam & (int)VehicleFlag.NO_DEFLECTION_UP) == (int)VehicleFlag.NO_DEFLECTION_UP) | ||
330 | { | ||
331 | if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) != (VehicleFlag)0) | ||
332 | m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP); | ||
333 | } | ||
334 | if ((pParam & (int)VehicleFlag.CAMERA_DECOUPLED) == (int)VehicleFlag.CAMERA_DECOUPLED) | ||
335 | { | ||
336 | if ((m_flags & VehicleFlag.CAMERA_DECOUPLED) != (VehicleFlag)0) | ||
337 | m_flags &= ~(VehicleFlag.CAMERA_DECOUPLED); | ||
338 | } | ||
339 | if ((pParam & (int)VehicleFlag.NO_X) == (int)VehicleFlag.NO_X) | ||
340 | { | ||
341 | if ((m_flags & VehicleFlag.NO_X) != (VehicleFlag)0) | ||
342 | m_flags &= ~(VehicleFlag.NO_X); | ||
343 | } | ||
344 | if ((pParam & (int)VehicleFlag.NO_Y) == (int)VehicleFlag.NO_Y) | ||
345 | { | ||
346 | if ((m_flags & VehicleFlag.NO_Y) != (VehicleFlag)0) | ||
347 | m_flags &= ~(VehicleFlag.NO_Y); | ||
348 | } | ||
349 | if ((pParam & (int)VehicleFlag.NO_Z) == (int)VehicleFlag.NO_Z) | ||
350 | { | ||
351 | if ((m_flags & VehicleFlag.NO_Z) != (VehicleFlag)0) | ||
352 | m_flags &= ~(VehicleFlag.NO_Z); | ||
353 | } | ||
354 | if ((pParam & (int)VehicleFlag.LOCK_HOVER_HEIGHT) == (int)VehicleFlag.LOCK_HOVER_HEIGHT) | ||
355 | { | ||
356 | if ((m_Hoverflags & VehicleFlag.LOCK_HOVER_HEIGHT) != (VehicleFlag)0) | ||
357 | m_Hoverflags &= ~(VehicleFlag.LOCK_HOVER_HEIGHT); | ||
358 | } | ||
359 | if ((pParam & (int)VehicleFlag.NO_DEFLECTION) == (int)VehicleFlag.NO_DEFLECTION) | ||
360 | { | ||
361 | if ((m_flags & VehicleFlag.NO_DEFLECTION) != (VehicleFlag)0) | ||
362 | m_flags &= ~(VehicleFlag.NO_DEFLECTION); | ||
363 | } | ||
364 | if ((pParam & (int)VehicleFlag.LOCK_ROTATION) == (int)VehicleFlag.LOCK_ROTATION) | ||
365 | { | ||
366 | if ((m_flags & VehicleFlag.LOCK_ROTATION) != (VehicleFlag)0) | ||
367 | m_flags &= ~(VehicleFlag.LOCK_ROTATION); | ||
368 | } | ||
369 | } | ||
370 | else | ||
371 | { | ||
372 | if ((pParam & (int)VehicleFlag.HOVER_GLOBAL_HEIGHT) == (int)VehicleFlag.HOVER_GLOBAL_HEIGHT) | ||
373 | { | ||
374 | m_Hoverflags |= (VehicleFlag.HOVER_GLOBAL_HEIGHT | m_flags); | ||
375 | } | ||
376 | if ((pParam & (int)VehicleFlag.HOVER_TERRAIN_ONLY) == (int)VehicleFlag.HOVER_TERRAIN_ONLY) | ||
377 | { | ||
378 | m_Hoverflags |= (VehicleFlag.HOVER_TERRAIN_ONLY | m_flags); | ||
379 | } | ||
380 | if ((pParam & (int)VehicleFlag.HOVER_UP_ONLY) == (int)VehicleFlag.HOVER_UP_ONLY) | ||
381 | { | ||
382 | m_Hoverflags |= (VehicleFlag.HOVER_UP_ONLY | m_flags); | ||
383 | } | ||
384 | if ((pParam & (int)VehicleFlag.HOVER_WATER_ONLY) == (int)VehicleFlag.HOVER_WATER_ONLY) | ||
385 | { | ||
386 | m_Hoverflags |= (VehicleFlag.HOVER_WATER_ONLY | m_flags); | ||
387 | } | ||
388 | if ((pParam & (int)VehicleFlag.LIMIT_MOTOR_UP) == (int)VehicleFlag.LIMIT_MOTOR_UP) | ||
389 | { | ||
390 | m_flags |= (VehicleFlag.LIMIT_MOTOR_UP | m_flags); | ||
391 | } | ||
392 | if ((pParam & (int)VehicleFlag.MOUSELOOK_BANK) == (int)VehicleFlag.MOUSELOOK_BANK) | ||
393 | { | ||
394 | m_flags |= (VehicleFlag.MOUSELOOK_BANK | m_flags); | ||
395 | } | ||
396 | if ((pParam & (int)VehicleFlag.MOUSELOOK_STEER) == (int)VehicleFlag.MOUSELOOK_STEER) | ||
397 | { | ||
398 | m_flags |= (VehicleFlag.MOUSELOOK_STEER | m_flags); | ||
399 | } | ||
400 | if ((pParam & (int)VehicleFlag.NO_DEFLECTION_UP) == (int)VehicleFlag.NO_DEFLECTION_UP) | ||
401 | { | ||
402 | m_flags |= (VehicleFlag.NO_DEFLECTION_UP | m_flags); | ||
403 | } | ||
404 | if ((pParam & (int)VehicleFlag.CAMERA_DECOUPLED) == (int)VehicleFlag.CAMERA_DECOUPLED) | ||
405 | { | ||
406 | m_flags |= (VehicleFlag.CAMERA_DECOUPLED | m_flags); | ||
407 | } | ||
408 | if ((pParam & (int)VehicleFlag.NO_X) == (int)VehicleFlag.NO_X) | ||
409 | { | ||
410 | m_flags |= (VehicleFlag.NO_X); | ||
411 | } | ||
412 | if ((pParam & (int)VehicleFlag.NO_Y) == (int)VehicleFlag.NO_Y) | ||
413 | { | ||
414 | m_flags |= (VehicleFlag.NO_Y); | ||
415 | } | ||
416 | if ((pParam & (int)VehicleFlag.NO_Z) == (int)VehicleFlag.NO_Z) | ||
417 | { | ||
418 | m_flags |= (VehicleFlag.NO_Z); | ||
419 | } | ||
420 | if ((pParam & (int)VehicleFlag.LOCK_HOVER_HEIGHT) == (int)VehicleFlag.LOCK_HOVER_HEIGHT) | ||
421 | { | ||
422 | m_Hoverflags |= (VehicleFlag.LOCK_HOVER_HEIGHT); | ||
423 | } | ||
424 | if ((pParam & (int)VehicleFlag.NO_DEFLECTION) == (int)VehicleFlag.NO_DEFLECTION) | ||
425 | { | ||
426 | m_flags |= (VehicleFlag.NO_DEFLECTION); | ||
427 | } | ||
428 | if ((pParam & (int)VehicleFlag.LOCK_ROTATION) == (int)VehicleFlag.LOCK_ROTATION) | ||
429 | { | ||
430 | m_flags |= (VehicleFlag.LOCK_ROTATION); | ||
431 | } | ||
432 | } | ||
433 | }//end ProcessVehicleFlags | ||
434 | |||
435 | internal void ProcessTypeChange(Vehicle pType) | ||
436 | { | ||
437 | // Set Defaults For Type | ||
438 | m_type = pType; | ||
439 | switch (pType) | ||
440 | { | ||
441 | case Vehicle.TYPE_NONE: | ||
442 | m_linearFrictionTimescale = new Vector3(0, 0, 0); | ||
443 | m_angularFrictionTimescale = new Vector3(0, 0, 0); | ||
444 | m_linearMotorDirection = Vector3.Zero; | ||
445 | m_linearMotorTimescale = 0; | ||
446 | m_linearMotorDecayTimescale = 0; | ||
447 | m_angularMotorDirection = Vector3.Zero; | ||
448 | m_angularMotorTimescale = 0; | ||
449 | m_angularMotorDecayTimescale = 0; | ||
450 | m_VhoverHeight = 0; | ||
451 | m_VhoverTimescale = 0; | ||
452 | m_VehicleBuoyancy = 0; | ||
453 | m_flags = (VehicleFlag)0; | ||
454 | break; | ||
455 | |||
456 | case Vehicle.TYPE_SLED: | ||
457 | m_linearFrictionTimescale = new Vector3(30, 1, 1000); | ||
458 | m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); | ||
459 | m_linearMotorDirection = Vector3.Zero; | ||
460 | m_linearMotorTimescale = 1000; | ||
461 | m_linearMotorDecayTimescale = 120; | ||
462 | m_angularMotorDirection = Vector3.Zero; | ||
463 | m_angularMotorTimescale = 1000; | ||
464 | m_angularMotorDecayTimescale = 120; | ||
465 | m_VhoverHeight = 0; | ||
466 | // m_VhoverEfficiency = 1; | ||
467 | m_VhoverTimescale = 10; | ||
468 | m_VehicleBuoyancy = 0; | ||
469 | // m_linearDeflectionEfficiency = 1; | ||
470 | // m_linearDeflectionTimescale = 1; | ||
471 | // m_angularDeflectionEfficiency = 1; | ||
472 | // m_angularDeflectionTimescale = 1000; | ||
473 | // m_bankingEfficiency = 0; | ||
474 | // m_bankingMix = 1; | ||
475 | // m_bankingTimescale = 10; | ||
476 | // m_referenceFrame = Quaternion.Identity; | ||
477 | m_Hoverflags &= | ||
478 | ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | | ||
479 | VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); | ||
480 | m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP); | ||
481 | break; | ||
482 | case Vehicle.TYPE_CAR: | ||
483 | m_linearFrictionTimescale = new Vector3(100, 2, 1000); | ||
484 | m_angularFrictionTimescale = new Vector3(1000, 1000, 1000); | ||
485 | m_linearMotorDirection = Vector3.Zero; | ||
486 | m_linearMotorTimescale = 1; | ||
487 | m_linearMotorDecayTimescale = 60; | ||
488 | m_angularMotorDirection = Vector3.Zero; | ||
489 | m_angularMotorTimescale = 1; | ||
490 | m_angularMotorDecayTimescale = 0.8f; | ||
491 | m_VhoverHeight = 0; | ||
492 | // m_VhoverEfficiency = 0; | ||
493 | m_VhoverTimescale = 1000; | ||
494 | m_VehicleBuoyancy = 0; | ||
495 | // // m_linearDeflectionEfficiency = 1; | ||
496 | // // m_linearDeflectionTimescale = 2; | ||
497 | // // m_angularDeflectionEfficiency = 0; | ||
498 | // m_angularDeflectionTimescale = 10; | ||
499 | m_verticalAttractionEfficiency = 1f; | ||
500 | m_verticalAttractionTimescale = 10f; | ||
501 | // m_bankingEfficiency = -0.2f; | ||
502 | // m_bankingMix = 1; | ||
503 | // m_bankingTimescale = 1; | ||
504 | // m_referenceFrame = Quaternion.Identity; | ||
505 | m_Hoverflags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT); | ||
506 | m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | | ||
507 | VehicleFlag.LIMIT_MOTOR_UP); | ||
508 | m_Hoverflags |= (VehicleFlag.HOVER_UP_ONLY); | ||
509 | break; | ||
510 | case Vehicle.TYPE_BOAT: | ||
511 | m_linearFrictionTimescale = new Vector3(10, 3, 2); | ||
512 | m_angularFrictionTimescale = new Vector3(10,10,10); | ||
513 | m_linearMotorDirection = Vector3.Zero; | ||
514 | m_linearMotorTimescale = 5; | ||
515 | m_linearMotorDecayTimescale = 60; | ||
516 | m_angularMotorDirection = Vector3.Zero; | ||
517 | m_angularMotorTimescale = 4; | ||
518 | m_angularMotorDecayTimescale = 4; | ||
519 | m_VhoverHeight = 0; | ||
520 | // m_VhoverEfficiency = 0.5f; | ||
521 | m_VhoverTimescale = 2; | ||
522 | m_VehicleBuoyancy = 1; | ||
523 | // m_linearDeflectionEfficiency = 0.5f; | ||
524 | // m_linearDeflectionTimescale = 3; | ||
525 | // m_angularDeflectionEfficiency = 0.5f; | ||
526 | // m_angularDeflectionTimescale = 5; | ||
527 | m_verticalAttractionEfficiency = 0.5f; | ||
528 | m_verticalAttractionTimescale = 5f; | ||
529 | // m_bankingEfficiency = -0.3f; | ||
530 | // m_bankingMix = 0.8f; | ||
531 | // m_bankingTimescale = 1; | ||
532 | // m_referenceFrame = Quaternion.Identity; | ||
533 | m_Hoverflags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | | ||
534 | VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); | ||
535 | m_flags &= ~(VehicleFlag.LIMIT_ROLL_ONLY); | ||
536 | m_flags |= (VehicleFlag.NO_DEFLECTION_UP | | ||
537 | VehicleFlag.LIMIT_MOTOR_UP); | ||
538 | m_Hoverflags |= (VehicleFlag.HOVER_WATER_ONLY); | ||
539 | break; | ||
540 | case Vehicle.TYPE_AIRPLANE: | ||
541 | m_linearFrictionTimescale = new Vector3(200, 10, 5); | ||
542 | m_angularFrictionTimescale = new Vector3(20, 20, 20); | ||
543 | m_linearMotorDirection = Vector3.Zero; | ||
544 | m_linearMotorTimescale = 2; | ||
545 | m_linearMotorDecayTimescale = 60; | ||
546 | m_angularMotorDirection = Vector3.Zero; | ||
547 | m_angularMotorTimescale = 4; | ||
548 | m_angularMotorDecayTimescale = 4; | ||
549 | m_VhoverHeight = 0; | ||
550 | // m_VhoverEfficiency = 0.5f; | ||
551 | m_VhoverTimescale = 1000; | ||
552 | m_VehicleBuoyancy = 0; | ||
553 | // m_linearDeflectionEfficiency = 0.5f; | ||
554 | // m_linearDeflectionTimescale = 3; | ||
555 | // m_angularDeflectionEfficiency = 1; | ||
556 | // m_angularDeflectionTimescale = 2; | ||
557 | m_verticalAttractionEfficiency = 0.9f; | ||
558 | m_verticalAttractionTimescale = 2f; | ||
559 | // m_bankingEfficiency = 1; | ||
560 | // m_bankingMix = 0.7f; | ||
561 | // m_bankingTimescale = 2; | ||
562 | // m_referenceFrame = Quaternion.Identity; | ||
563 | m_Hoverflags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | | ||
564 | VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY); | ||
565 | m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_MOTOR_UP); | ||
566 | m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); | ||
567 | break; | ||
568 | case Vehicle.TYPE_BALLOON: | ||
569 | m_linearFrictionTimescale = new Vector3(5, 5, 5); | ||
570 | m_angularFrictionTimescale = new Vector3(10, 10, 10); | ||
571 | m_linearMotorDirection = Vector3.Zero; | ||
572 | m_linearMotorTimescale = 5; | ||
573 | m_linearMotorDecayTimescale = 60; | ||
574 | m_angularMotorDirection = Vector3.Zero; | ||
575 | m_angularMotorTimescale = 6; | ||
576 | m_angularMotorDecayTimescale = 10; | ||
577 | m_VhoverHeight = 5; | ||
578 | // m_VhoverEfficiency = 0.8f; | ||
579 | m_VhoverTimescale = 10; | ||
580 | m_VehicleBuoyancy = 1; | ||
581 | // m_linearDeflectionEfficiency = 0; | ||
582 | // m_linearDeflectionTimescale = 5; | ||
583 | // m_angularDeflectionEfficiency = 0; | ||
584 | // m_angularDeflectionTimescale = 5; | ||
585 | m_verticalAttractionEfficiency = 1f; | ||
586 | m_verticalAttractionTimescale = 100f; | ||
587 | // m_bankingEfficiency = 0; | ||
588 | // m_bankingMix = 0.7f; | ||
589 | // m_bankingTimescale = 5; | ||
590 | // m_referenceFrame = Quaternion.Identity; | ||
591 | m_Hoverflags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | | ||
592 | VehicleFlag.HOVER_UP_ONLY); | ||
593 | m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_MOTOR_UP); | ||
594 | m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY); | ||
595 | m_Hoverflags |= (VehicleFlag.HOVER_GLOBAL_HEIGHT); | ||
596 | break; | ||
597 | |||
598 | } | ||
599 | }//end SetDefaultsForType | ||
600 | |||
601 | internal void Step(float pTimestep, BSScene pParentScene) | ||
602 | { | ||
603 | if (m_type == Vehicle.TYPE_NONE) return; | ||
604 | |||
605 | frcount++; // used to limit debug comment output | ||
606 | if (frcount > 100) | ||
607 | frcount = 0; | ||
608 | |||
609 | MoveLinear(pTimestep, pParentScene); | ||
610 | MoveAngular(pTimestep); | ||
611 | LimitRotation(pTimestep); | ||
612 | }// end Step | ||
613 | |||
614 | private void MoveLinear(float pTimestep, BSScene _pParentScene) | ||
615 | { | ||
616 | if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant | ||
617 | { | ||
618 | // add drive to body | ||
619 | Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep); | ||
620 | m_lastLinearVelocityVector += (addAmount*10); // lastLinearVelocityVector is the current body velocity vector? | ||
621 | |||
622 | // This will work temporarily, but we really need to compare speed on an axis | ||
623 | // KF: Limit body velocity to applied velocity? | ||
624 | if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X)) | ||
625 | m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X; | ||
626 | if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y)) | ||
627 | m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y; | ||
628 | if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z)) | ||
629 | m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z; | ||
630 | |||
631 | // decay applied velocity | ||
632 | Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep))); | ||
633 | //Console.WriteLine("decay: " + decayfraction); | ||
634 | m_linearMotorDirection -= m_linearMotorDirection * decayfraction * 0.5f; | ||
635 | //Console.WriteLine("actual: " + m_linearMotorDirection); | ||
636 | } | ||
637 | else | ||
638 | { // requested is not significant | ||
639 | // if what remains of applied is small, zero it. | ||
640 | if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f)) | ||
641 | m_lastLinearVelocityVector = Vector3.Zero; | ||
642 | } | ||
643 | |||
644 | // convert requested object velocity to world-referenced vector | ||
645 | m_dir = m_lastLinearVelocityVector; | ||
646 | Quaternion rot = m_prim.Orientation; | ||
647 | Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object | ||
648 | m_dir *= rotq; // apply obj rotation to velocity vector | ||
649 | |||
650 | // add Gravity andBuoyancy | ||
651 | // KF: So far I have found no good method to combine a script-requested | ||
652 | // .Z velocity and gravity. Therefore only 0g will used script-requested | ||
653 | // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only. | ||
654 | Vector3 grav = Vector3.Zero; | ||
655 | // There is some gravity, make a gravity force vector | ||
656 | // that is applied after object velocity. | ||
657 | float objMass = m_prim.Mass; | ||
658 | // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g; | ||
659 | grav.Z = _pParentScene.DefaultGravity.Z * objMass * (1f - m_VehicleBuoyancy); | ||
660 | // Preserve the current Z velocity | ||
661 | Vector3 vel_now = m_prim.Velocity; | ||
662 | m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity | ||
663 | |||
664 | Vector3 pos = m_prim.Position; | ||
665 | // Vector3 accel = new Vector3(-(m_dir.X - m_lastLinearVelocityVector.X / 0.1f), -(m_dir.Y - m_lastLinearVelocityVector.Y / 0.1f), m_dir.Z - m_lastLinearVelocityVector.Z / 0.1f); | ||
666 | Vector3 posChange = new Vector3(); | ||
667 | posChange.X = pos.X - m_lastPositionVector.X; | ||
668 | posChange.Y = pos.Y - m_lastPositionVector.Y; | ||
669 | posChange.Z = pos.Z - m_lastPositionVector.Z; | ||
670 | double Zchange = Math.Abs(posChange.Z); | ||
671 | if (m_BlockingEndPoint != Vector3.Zero) | ||
672 | { | ||
673 | if (pos.X >= (m_BlockingEndPoint.X - (float)1)) | ||
674 | { | ||
675 | pos.X -= posChange.X + 1; | ||
676 | m_prim.Position = pos; | ||
677 | } | ||
678 | if (pos.Y >= (m_BlockingEndPoint.Y - (float)1)) | ||
679 | { | ||
680 | pos.Y -= posChange.Y + 1; | ||
681 | m_prim.Position = pos; | ||
682 | } | ||
683 | if (pos.Z >= (m_BlockingEndPoint.Z - (float)1)) | ||
684 | { | ||
685 | pos.Z -= posChange.Z + 1; | ||
686 | m_prim.Position = pos; | ||
687 | } | ||
688 | if (pos.X <= 0) | ||
689 | { | ||
690 | pos.X += posChange.X + 1; | ||
691 | m_prim.Position = pos; | ||
692 | } | ||
693 | if (pos.Y <= 0) | ||
694 | { | ||
695 | pos.Y += posChange.Y + 1; | ||
696 | m_prim.Position = pos; | ||
697 | } | ||
698 | } | ||
699 | if (pos.Z < _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y)) | ||
700 | { | ||
701 | pos.Z = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + 2; | ||
702 | m_prim.Position = pos; | ||
703 | } | ||
704 | |||
705 | // Check if hovering | ||
706 | if ((m_Hoverflags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0) | ||
707 | { | ||
708 | // We should hover, get the target height | ||
709 | if ((m_Hoverflags & VehicleFlag.HOVER_WATER_ONLY) != 0) | ||
710 | { | ||
711 | m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight; | ||
712 | } | ||
713 | if ((m_Hoverflags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0) | ||
714 | { | ||
715 | m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight; | ||
716 | } | ||
717 | if ((m_Hoverflags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0) | ||
718 | { | ||
719 | m_VhoverTargetHeight = m_VhoverHeight; | ||
720 | } | ||
721 | |||
722 | if ((m_Hoverflags & VehicleFlag.HOVER_UP_ONLY) != 0) | ||
723 | { | ||
724 | // If body is aready heigher, use its height as target height | ||
725 | if (pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z; | ||
726 | } | ||
727 | if ((m_Hoverflags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0) | ||
728 | { | ||
729 | if ((pos.Z - m_VhoverTargetHeight) > .2 || (pos.Z - m_VhoverTargetHeight) < -.2) | ||
730 | { | ||
731 | m_prim.Position = pos; | ||
732 | } | ||
733 | } | ||
734 | else | ||
735 | { | ||
736 | float herr0 = pos.Z - m_VhoverTargetHeight; | ||
737 | // Replace Vertical speed with correction figure if significant | ||
738 | if (Math.Abs(herr0) > 0.01f) | ||
739 | { | ||
740 | m_dir.Z = -((herr0 * pTimestep * 50.0f) / m_VhoverTimescale); | ||
741 | //KF: m_VhoverEfficiency is not yet implemented | ||
742 | } | ||
743 | else | ||
744 | { | ||
745 | m_dir.Z = 0f; | ||
746 | } | ||
747 | } | ||
748 | |||
749 | // m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped | ||
750 | // m_VhoverTimescale = 0f; // time to acheive height | ||
751 | // pTimestep is time since last frame,in secs | ||
752 | } | ||
753 | |||
754 | if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0) | ||
755 | { | ||
756 | //Start Experimental Values | ||
757 | if (Zchange > .3) | ||
758 | { | ||
759 | grav.Z = (float)(grav.Z * 3); | ||
760 | } | ||
761 | if (Zchange > .15) | ||
762 | { | ||
763 | grav.Z = (float)(grav.Z * 2); | ||
764 | } | ||
765 | if (Zchange > .75) | ||
766 | { | ||
767 | grav.Z = (float)(grav.Z * 1.5); | ||
768 | } | ||
769 | if (Zchange > .05) | ||
770 | { | ||
771 | grav.Z = (float)(grav.Z * 1.25); | ||
772 | } | ||
773 | if (Zchange > .025) | ||
774 | { | ||
775 | grav.Z = (float)(grav.Z * 1.125); | ||
776 | } | ||
777 | float terraintemp = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y); | ||
778 | float postemp = (pos.Z - terraintemp); | ||
779 | if (postemp > 2.5f) | ||
780 | { | ||
781 | grav.Z = (float)(grav.Z * 1.037125); | ||
782 | } | ||
783 | //End Experimental Values | ||
784 | } | ||
785 | if ((m_flags & (VehicleFlag.NO_X)) != 0) | ||
786 | { | ||
787 | m_dir.X = 0; | ||
788 | } | ||
789 | if ((m_flags & (VehicleFlag.NO_Y)) != 0) | ||
790 | { | ||
791 | m_dir.Y = 0; | ||
792 | } | ||
793 | if ((m_flags & (VehicleFlag.NO_Z)) != 0) | ||
794 | { | ||
795 | m_dir.Z = 0; | ||
796 | } | ||
797 | |||
798 | m_lastPositionVector = m_prim.Position; | ||
799 | |||
800 | // Apply velocity | ||
801 | m_prim.Velocity = m_dir; | ||
802 | // apply gravity force | ||
803 | m_prim.Force = grav; | ||
804 | |||
805 | |||
806 | // apply friction | ||
807 | Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep); | ||
808 | m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount; | ||
809 | } // end MoveLinear() | ||
810 | |||
811 | private void MoveAngular(float pTimestep) | ||
812 | { | ||
813 | /* | ||
814 | private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor | ||
815 | private int m_angularMotorApply = 0; // application frame counter | ||
816 | private float m_angularMotorVelocity = 0; // current angular motor velocity (ramps up and down) | ||
817 | private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate | ||
818 | private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate | ||
819 | private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate | ||
820 | private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body | ||
821 | */ | ||
822 | |||
823 | // Get what the body is doing, this includes 'external' influences | ||
824 | Vector3 angularVelocity = m_prim.AngularVelocity; | ||
825 | // Vector3 angularVelocity = Vector3.Zero; | ||
826 | |||
827 | if (m_angularMotorApply > 0) | ||
828 | { | ||
829 | // ramp up to new value | ||
830 | // current velocity += error / (time to get there / step interval) | ||
831 | // requested speed - last motor speed | ||
832 | m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep); | ||
833 | m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep); | ||
834 | m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep); | ||
835 | |||
836 | m_angularMotorApply--; // This is done so that if script request rate is less than phys frame rate the expected | ||
837 | // velocity may still be acheived. | ||
838 | } | ||
839 | else | ||
840 | { | ||
841 | // no motor recently applied, keep the body velocity | ||
842 | /* m_angularMotorVelocity.X = angularVelocity.X; | ||
843 | m_angularMotorVelocity.Y = angularVelocity.Y; | ||
844 | m_angularMotorVelocity.Z = angularVelocity.Z; */ | ||
845 | |||
846 | // and decay the velocity | ||
847 | m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / pTimestep); | ||
848 | } // end motor section | ||
849 | |||
850 | // Vertical attractor section | ||
851 | Vector3 vertattr = Vector3.Zero; | ||
852 | |||
853 | if (m_verticalAttractionTimescale < 300) | ||
854 | { | ||
855 | float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep); | ||
856 | // get present body rotation | ||
857 | Quaternion rotq = m_prim.Orientation; | ||
858 | // make a vector pointing up | ||
859 | Vector3 verterr = Vector3.Zero; | ||
860 | verterr.Z = 1.0f; | ||
861 | // rotate it to Body Angle | ||
862 | verterr = verterr * rotq; | ||
863 | // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1. | ||
864 | // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go | ||
865 | // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body. | ||
866 | if (verterr.Z < 0.0f) | ||
867 | { | ||
868 | verterr.X = 2.0f - verterr.X; | ||
869 | verterr.Y = 2.0f - verterr.Y; | ||
870 | } | ||
871 | // Error is 0 (no error) to +/- 2 (max error) | ||
872 | // scale it by VAservo | ||
873 | verterr = verterr * VAservo; | ||
874 | //if (frcount == 0) Console.WriteLine("VAerr=" + verterr); | ||
875 | |||
876 | // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so | ||
877 | // Change Body angular velocity X based on Y, and Y based on X. Z is not changed. | ||
878 | vertattr.X = verterr.Y; | ||
879 | vertattr.Y = - verterr.X; | ||
880 | vertattr.Z = 0f; | ||
881 | |||
882 | // scaling appears better usingsquare-law | ||
883 | float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency); | ||
884 | vertattr.X += bounce * angularVelocity.X; | ||
885 | vertattr.Y += bounce * angularVelocity.Y; | ||
886 | |||
887 | } // else vertical attractor is off | ||
888 | |||
889 | // m_lastVertAttractor = vertattr; | ||
890 | |||
891 | // Bank section tba | ||
892 | // Deflection section tba | ||
893 | |||
894 | // Sum velocities | ||
895 | m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // + bank + deflection | ||
896 | |||
897 | if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0) | ||
898 | { | ||
899 | m_lastAngularVelocity.X = 0; | ||
900 | m_lastAngularVelocity.Y = 0; | ||
901 | } | ||
902 | |||
903 | if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f)) | ||
904 | { | ||
905 | m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero. | ||
906 | } | ||
907 | |||
908 | // apply friction | ||
909 | Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep); | ||
910 | m_lastAngularVelocity -= m_lastAngularVelocity * decayamount; | ||
911 | |||
912 | // Apply to the body | ||
913 | m_prim.AngularVelocity = m_lastAngularVelocity; | ||
914 | |||
915 | } //end MoveAngular | ||
916 | internal void LimitRotation(float timestep) | ||
917 | { | ||
918 | Quaternion rotq = m_prim.Orientation; // rotq = rotation of object | ||
919 | Quaternion m_rot = rotq; | ||
920 | bool changed = false; | ||
921 | if (m_RollreferenceFrame != Quaternion.Identity) | ||
922 | { | ||
923 | if (rotq.X >= m_RollreferenceFrame.X) | ||
924 | { | ||
925 | m_rot.X = rotq.X - (m_RollreferenceFrame.X / 2); | ||
926 | } | ||
927 | if (rotq.Y >= m_RollreferenceFrame.Y) | ||
928 | { | ||
929 | m_rot.Y = rotq.Y - (m_RollreferenceFrame.Y / 2); | ||
930 | } | ||
931 | if (rotq.X <= -m_RollreferenceFrame.X) | ||
932 | { | ||
933 | m_rot.X = rotq.X + (m_RollreferenceFrame.X / 2); | ||
934 | } | ||
935 | if (rotq.Y <= -m_RollreferenceFrame.Y) | ||
936 | { | ||
937 | m_rot.Y = rotq.Y + (m_RollreferenceFrame.Y / 2); | ||
938 | } | ||
939 | changed = true; | ||
940 | } | ||
941 | if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0) | ||
942 | { | ||
943 | m_rot.X = 0; | ||
944 | m_rot.Y = 0; | ||
945 | changed = true; | ||
946 | } | ||
947 | if (changed) | ||
948 | m_prim.Orientation = m_rot; | ||
949 | } | ||
950 | } | ||
951 | } | ||
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPlugin.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPlugin.cs new file mode 100644 index 0000000..61be56d --- /dev/null +++ b/OpenSim/Region/Physics/BulletSPlugin/BSPlugin.cs | |||
@@ -0,0 +1,68 @@ | |||
1 | /* | ||
2 | * Copyright (c) Contributors, http://opensimulator.org/ | ||
3 | * See CONTRIBUTORS.TXT for a full list of copyright holders. | ||
4 | * | ||
5 | * Redistribution and use in source and binary forms, with or without | ||
6 | * modification, are permitted provided that the following conditions are met: | ||
7 | * * Redistributions of source code must retain the above copyright | ||
8 | * notice, this list of conditions and the following disclaimer. | ||
9 | * * Redistributions in binary form must reproduce the above copyrightD | ||
10 | * notice, this list of conditions and the following disclaimer in the | ||
11 | * documentation and/or other materials provided with the distribution. | ||
12 | * * Neither the name of the OpenSimulator Project nor the | ||
13 | * names of its contributors may be used to endorse or promote products | ||
14 | * derived from this software without specific prior written permission. | ||
15 | * | ||
16 | * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY | ||
17 | * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED | ||
18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE | ||
19 | * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY | ||
20 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES | ||
21 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
22 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND | ||
23 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
24 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS | ||
25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. | ||
26 | */ | ||
27 | using System; | ||
28 | using System.Collections.Generic; | ||
29 | using OpenSim.Framework; | ||
30 | using OpenSim.Region.Physics.Manager; | ||
31 | using OpenMetaverse; | ||
32 | |||
33 | namespace OpenSim.Region.Physics.BulletSPlugin | ||
34 | { | ||
35 | public class BSPlugin : IPhysicsPlugin | ||
36 | { | ||
37 | //private static readonly log4net.ILog m_log = log4net.LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType); | ||
38 | |||
39 | private BSScene _mScene; | ||
40 | |||
41 | public BSPlugin() | ||
42 | { | ||
43 | } | ||
44 | |||
45 | public bool Init() | ||
46 | { | ||
47 | return true; | ||
48 | } | ||
49 | |||
50 | public PhysicsScene GetScene(String sceneIdentifier) | ||
51 | { | ||
52 | if (_mScene == null) | ||
53 | { | ||
54 | _mScene = new BSScene(sceneIdentifier); | ||
55 | } | ||
56 | return (_mScene); | ||
57 | } | ||
58 | |||
59 | public string GetName() | ||
60 | { | ||
61 | return ("BulletSim"); | ||
62 | } | ||
63 | |||
64 | public void Dispose() | ||
65 | { | ||
66 | } | ||
67 | } | ||
68 | } | ||
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs new file mode 100644 index 0000000..04cb452 --- /dev/null +++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs | |||
@@ -0,0 +1,1265 @@ | |||
1 | /* | ||
2 | * Copyright (c) Contributors, http://opensimulator.org/ | ||
3 | * See CONTRIBUTORS.TXT for a full list of copyright holders. | ||
4 | * | ||
5 | * Redistribution and use in source and binary forms, with or without | ||
6 | * modification, are permitted provided that the following conditions are met: | ||
7 | * * Redistributions of source code must retain the above copyright | ||
8 | * notice, this list of conditions and the following disclaimer. | ||
9 | * * Redistributions in binary form must reproduce the above copyrightD | ||
10 | * notice, this list of conditions and the following disclaimer in the | ||
11 | * documentation and/or other materials provided with the distribution. | ||
12 | * * Neither the name of the OpenSimulator Project nor the | ||
13 | * names of its contributors may be used to endorse or promote products | ||
14 | * derived from this software without specific prior written permission. | ||
15 | * | ||
16 | * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY | ||
17 | * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED | ||
18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE | ||
19 | * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY | ||
20 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES | ||
21 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
22 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND | ||
23 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
24 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS | ||
25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. | ||
26 | */ | ||
27 | using System; | ||
28 | using System.Reflection; | ||
29 | using System.Collections.Generic; | ||
30 | using System.Xml; | ||
31 | using log4net; | ||
32 | using OMV = OpenMetaverse; | ||
33 | using OpenSim.Framework; | ||
34 | using OpenSim.Region.Physics.Manager; | ||
35 | using OpenSim.Region.Physics.ConvexDecompositionDotNet; | ||
36 | |||
37 | namespace OpenSim.Region.Physics.BulletSPlugin | ||
38 | { | ||
39 | [Serializable] | ||
40 | public sealed class BSPrim : PhysicsActor | ||
41 | { | ||
42 | private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType); | ||
43 | private static readonly string LogHeader = "[BULLETS PRIM]"; | ||
44 | |||
45 | private IMesh _mesh; | ||
46 | private PrimitiveBaseShape _pbs; | ||
47 | private ShapeData.PhysicsShapeType _shapeType; | ||
48 | private ulong _hullKey; | ||
49 | private List<ConvexResult> _hulls; | ||
50 | |||
51 | private BSScene _scene; | ||
52 | private String _avName; | ||
53 | private uint _localID = 0; | ||
54 | |||
55 | private OMV.Vector3 _size; | ||
56 | private OMV.Vector3 _scale; | ||
57 | private bool _stopped; | ||
58 | private bool _grabbed; | ||
59 | private bool _isSelected; | ||
60 | private bool _isVolumeDetect; | ||
61 | private OMV.Vector3 _position; | ||
62 | private float _mass; | ||
63 | private float _density; | ||
64 | private OMV.Vector3 _force; | ||
65 | private OMV.Vector3 _velocity; | ||
66 | private OMV.Vector3 _torque; | ||
67 | private float _collisionScore; | ||
68 | private OMV.Vector3 _acceleration; | ||
69 | private OMV.Quaternion _orientation; | ||
70 | private int _physicsActorType; | ||
71 | private bool _isPhysical; | ||
72 | private bool _flying; | ||
73 | private float _friction; | ||
74 | private float _restitution; | ||
75 | private bool _setAlwaysRun; | ||
76 | private bool _throttleUpdates; | ||
77 | private bool _isColliding; | ||
78 | private bool _collidingGround; | ||
79 | private bool _collidingObj; | ||
80 | private bool _floatOnWater; | ||
81 | private OMV.Vector3 _rotationalVelocity; | ||
82 | private bool _kinematic; | ||
83 | private float _buoyancy; | ||
84 | private OMV.Vector3 _angularVelocity; | ||
85 | |||
86 | private List<BSPrim> _childrenPrims; | ||
87 | private BSPrim _parentPrim; | ||
88 | |||
89 | private int _subscribedEventsMs = 0; | ||
90 | private int _lastCollisionTime = 0; | ||
91 | long _collidingStep; | ||
92 | long _collidingGroundStep; | ||
93 | |||
94 | private BSDynamics _vehicle; | ||
95 | |||
96 | private OMV.Vector3 _PIDTarget; | ||
97 | private bool _usePID; | ||
98 | private float _PIDTau; | ||
99 | private bool _useHoverPID; | ||
100 | private float _PIDHoverHeight; | ||
101 | private PIDHoverType _PIDHoverType; | ||
102 | private float _PIDHoverTao; | ||
103 | |||
104 | public BSPrim(uint localID, String primName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 size, | ||
105 | OMV.Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical) | ||
106 | { | ||
107 | // m_log.DebugFormat("{0}: BSPrim creation of {1}, id={2}", LogHeader, primName, localID); | ||
108 | _localID = localID; | ||
109 | _avName = primName; | ||
110 | _scene = parent_scene; | ||
111 | _position = pos; | ||
112 | _size = size; | ||
113 | _scale = new OMV.Vector3(1f, 1f, 1f); // the scale will be set by CreateGeom depending on object type | ||
114 | _orientation = rotation; | ||
115 | _buoyancy = 1f; | ||
116 | _velocity = OMV.Vector3.Zero; | ||
117 | _rotationalVelocity = OMV.Vector3.Zero; | ||
118 | _angularVelocity = OMV.Vector3.Zero; | ||
119 | _hullKey = 0; | ||
120 | _pbs = pbs; | ||
121 | _isPhysical = pisPhysical; | ||
122 | _isVolumeDetect = false; | ||
123 | _subscribedEventsMs = 0; | ||
124 | _friction = _scene.Params.defaultFriction; // TODO: compute based on object material | ||
125 | _density = _scene.Params.defaultDensity; // TODO: compute based on object material | ||
126 | _restitution = _scene.Params.defaultRestitution; | ||
127 | _parentPrim = null; // not a child or a parent | ||
128 | _vehicle = new BSDynamics(this); // add vehicleness | ||
129 | _childrenPrims = new List<BSPrim>(); | ||
130 | if (_isPhysical) | ||
131 | _mass = CalculateMass(); | ||
132 | else | ||
133 | _mass = 0f; | ||
134 | // do the actual object creation at taint time | ||
135 | _scene.TaintedObject(delegate() | ||
136 | { | ||
137 | RecreateGeomAndObject(); | ||
138 | }); | ||
139 | } | ||
140 | |||
141 | // called when this prim is being destroyed and we should free all the resources | ||
142 | public void Destroy() | ||
143 | { | ||
144 | // m_log.DebugFormat("{0}: Destroy", LogHeader); | ||
145 | // Undo any vehicle properties | ||
146 | _vehicle.ProcessTypeChange(Vehicle.TYPE_NONE); | ||
147 | _scene.RemoveVehiclePrim(this); // just to make sure | ||
148 | _scene.TaintedObject(delegate() | ||
149 | { | ||
150 | BulletSimAPI.DestroyObject(_scene.WorldID, _localID); | ||
151 | }); | ||
152 | } | ||
153 | |||
154 | public override bool Stopped { | ||
155 | get { return _stopped; } | ||
156 | } | ||
157 | public override OMV.Vector3 Size { | ||
158 | get { return _size; } | ||
159 | set { | ||
160 | _size = value; | ||
161 | _scene.TaintedObject(delegate() | ||
162 | { | ||
163 | if (_isPhysical) _mass = CalculateMass(); // changing size changes the mass | ||
164 | BulletSimAPI.SetObjectScaleMass(_scene.WorldID, _localID, _scale, _mass, _isPhysical); | ||
165 | RecreateGeomAndObject(); | ||
166 | }); | ||
167 | } | ||
168 | } | ||
169 | public override PrimitiveBaseShape Shape { | ||
170 | set { | ||
171 | _pbs = value; | ||
172 | _scene.TaintedObject(delegate() | ||
173 | { | ||
174 | if (_isPhysical) _mass = CalculateMass(); // changing the shape changes the mass | ||
175 | RecreateGeomAndObject(); | ||
176 | }); | ||
177 | } | ||
178 | } | ||
179 | public override uint LocalID { | ||
180 | set { _localID = value; } | ||
181 | get { return _localID; } | ||
182 | } | ||
183 | public override bool Grabbed { | ||
184 | set { _grabbed = value; | ||
185 | } | ||
186 | } | ||
187 | public override bool Selected { | ||
188 | set { | ||
189 | _isSelected = value; | ||
190 | _scene.TaintedObject(delegate() | ||
191 | { | ||
192 | SetObjectDynamic(); | ||
193 | }); | ||
194 | } | ||
195 | } | ||
196 | public override void CrossingFailure() { return; } | ||
197 | |||
198 | // link me to the specified parent | ||
199 | public override void link(PhysicsActor obj) { | ||
200 | BSPrim parent = (BSPrim)obj; | ||
201 | // m_log.DebugFormat("{0}: link {1}/{2} to {3}", LogHeader, _avName, _localID, obj.LocalID); | ||
202 | // TODO: decide if this parent checking needs to happen at taint time | ||
203 | if (_parentPrim == null) | ||
204 | { | ||
205 | if (parent != null) | ||
206 | { | ||
207 | // I don't have a parent so I am joining a linkset | ||
208 | parent.AddChildToLinkset(this); | ||
209 | } | ||
210 | } | ||
211 | else | ||
212 | { | ||
213 | // I already have a parent, is parenting changing? | ||
214 | if (parent != _parentPrim) | ||
215 | { | ||
216 | if (parent == null) | ||
217 | { | ||
218 | // we are being removed from a linkset | ||
219 | _parentPrim.RemoveChildFromLinkset(this); | ||
220 | } | ||
221 | else | ||
222 | { | ||
223 | // asking to reparent a prim should not happen | ||
224 | m_log.ErrorFormat("{0}: Reparenting a prim. ", LogHeader); | ||
225 | } | ||
226 | } | ||
227 | } | ||
228 | return; | ||
229 | } | ||
230 | |||
231 | // delink me from my linkset | ||
232 | public override void delink() { | ||
233 | // TODO: decide if this parent checking needs to happen at taint time | ||
234 | // Race condition here: if link() and delink() in same simulation tick, the delink will not happen | ||
235 | // m_log.DebugFormat("{0}: delink {1}/{2}", LogHeader, _avName, _localID); | ||
236 | if (_parentPrim != null) | ||
237 | { | ||
238 | _parentPrim.RemoveChildFromLinkset(this); | ||
239 | } | ||
240 | return; | ||
241 | } | ||
242 | |||
243 | // I am the root of a linkset and a new child is being added | ||
244 | public void AddChildToLinkset(BSPrim pchild) | ||
245 | { | ||
246 | BSPrim child = pchild; | ||
247 | _scene.TaintedObject(delegate() | ||
248 | { | ||
249 | if (!_childrenPrims.Contains(child)) | ||
250 | { | ||
251 | _childrenPrims.Add(child); | ||
252 | child.ParentPrim = this; // the child has gained a parent | ||
253 | RecreateGeomAndObject(); // rebuild my shape with the new child added | ||
254 | } | ||
255 | }); | ||
256 | return; | ||
257 | } | ||
258 | |||
259 | // I am the root of a linkset and one of my children is being removed. | ||
260 | // Safe to call even if the child is not really in my linkset. | ||
261 | public void RemoveChildFromLinkset(BSPrim pchild) | ||
262 | { | ||
263 | BSPrim child = pchild; | ||
264 | _scene.TaintedObject(delegate() | ||
265 | { | ||
266 | if (_childrenPrims.Contains(child)) | ||
267 | { | ||
268 | BulletSimAPI.RemoveConstraint(_scene.WorldID, child.LocalID, this.LocalID); | ||
269 | _childrenPrims.Remove(child); | ||
270 | child.ParentPrim = null; // the child has lost its parent | ||
271 | RecreateGeomAndObject(); // rebuild my shape with the child removed | ||
272 | } | ||
273 | else | ||
274 | { | ||
275 | m_log.ErrorFormat("{0}: Asked to remove child from linkset that was not in linkset"); | ||
276 | } | ||
277 | }); | ||
278 | return; | ||
279 | } | ||
280 | |||
281 | public BSPrim ParentPrim | ||
282 | { | ||
283 | set { _parentPrim = value; } | ||
284 | } | ||
285 | |||
286 | public ulong HullKey | ||
287 | { | ||
288 | get { return _hullKey; } | ||
289 | } | ||
290 | |||
291 | // return true if we are the root of a linkset (there are children to manage) | ||
292 | public bool IsRootOfLinkset | ||
293 | { | ||
294 | get { return (_parentPrim == null && _childrenPrims.Count != 0); } | ||
295 | } | ||
296 | |||
297 | // Set motion values to zero. | ||
298 | // Do it to the properties so the values get set in the physics engine. | ||
299 | // Push the setting of the values to the viewer. | ||
300 | private void ZeroMotion() | ||
301 | { | ||
302 | Velocity = OMV.Vector3.Zero; | ||
303 | _acceleration = OMV.Vector3.Zero; | ||
304 | RotationalVelocity = OMV.Vector3.Zero; | ||
305 | base.RequestPhysicsterseUpdate(); | ||
306 | } | ||
307 | |||
308 | public override void LockAngularMotion(OMV.Vector3 axis) { return; } | ||
309 | |||
310 | public override OMV.Vector3 Position { | ||
311 | get { | ||
312 | // don't do the following GetObjectPosition because this function is called a zillion times | ||
313 | // _position = BulletSimAPI.GetObjectPosition(_scene.WorldID, _localID); | ||
314 | return _position; | ||
315 | } | ||
316 | set { | ||
317 | _position = value; | ||
318 | _scene.TaintedObject(delegate() | ||
319 | { | ||
320 | BulletSimAPI.SetObjectTranslation(_scene.WorldID, _localID, _position, _orientation); | ||
321 | // m_log.DebugFormat("{0}: setPosition: id={1}, position={2}", LogHeader, _localID, _position); | ||
322 | }); | ||
323 | } | ||
324 | } | ||
325 | public override float Mass { | ||
326 | get { return _mass; } | ||
327 | } | ||
328 | public override OMV.Vector3 Force { | ||
329 | get { return _force; } | ||
330 | set { | ||
331 | _force = value; | ||
332 | _scene.TaintedObject(delegate() | ||
333 | { | ||
334 | BulletSimAPI.SetObjectForce(_scene.WorldID, _localID, _force); | ||
335 | }); | ||
336 | } | ||
337 | } | ||
338 | |||
339 | public override int VehicleType { | ||
340 | get { | ||
341 | return (int)_vehicle.Type; // if we are a vehicle, return that type | ||
342 | } | ||
343 | set { | ||
344 | Vehicle type = (Vehicle)value; | ||
345 | _vehicle.ProcessTypeChange(type); | ||
346 | _scene.TaintedObject(delegate() | ||
347 | { | ||
348 | if (type == Vehicle.TYPE_NONE) | ||
349 | { | ||
350 | _scene.RemoveVehiclePrim(this); | ||
351 | } | ||
352 | else | ||
353 | { | ||
354 | // make it so the scene will call us each tick to do vehicle things | ||
355 | _scene.AddVehiclePrim(this); | ||
356 | } | ||
357 | return; | ||
358 | }); | ||
359 | } | ||
360 | } | ||
361 | public override void VehicleFloatParam(int param, float value) | ||
362 | { | ||
363 | _vehicle.ProcessFloatVehicleParam((Vehicle)param, value); | ||
364 | } | ||
365 | public override void VehicleVectorParam(int param, OMV.Vector3 value) | ||
366 | { | ||
367 | _vehicle.ProcessVectorVehicleParam((Vehicle)param, value); | ||
368 | } | ||
369 | public override void VehicleRotationParam(int param, OMV.Quaternion rotation) | ||
370 | { | ||
371 | _vehicle.ProcessRotationVehicleParam((Vehicle)param, rotation); | ||
372 | } | ||
373 | public override void VehicleFlags(int param, bool remove) | ||
374 | { | ||
375 | _vehicle.ProcessVehicleFlags(param, remove); | ||
376 | } | ||
377 | // Called each simulation step to advance vehicle characteristics | ||
378 | public void StepVehicle(float timeStep) | ||
379 | { | ||
380 | _vehicle.Step(timeStep, _scene); | ||
381 | } | ||
382 | |||
383 | // Allows the detection of collisions with inherently non-physical prims. see llVolumeDetect for more | ||
384 | public override void SetVolumeDetect(int param) { | ||
385 | bool newValue = (param != 0); | ||
386 | if (_isVolumeDetect != newValue) | ||
387 | { | ||
388 | _isVolumeDetect = newValue; | ||
389 | _scene.TaintedObject(delegate() | ||
390 | { | ||
391 | SetObjectDynamic(); | ||
392 | }); | ||
393 | } | ||
394 | return; | ||
395 | } | ||
396 | |||
397 | public override OMV.Vector3 GeometricCenter { get { return OMV.Vector3.Zero; } } | ||
398 | public override OMV.Vector3 CenterOfMass { get { return OMV.Vector3.Zero; } } | ||
399 | public override OMV.Vector3 Velocity { | ||
400 | get { return _velocity; } | ||
401 | set { _velocity = value; | ||
402 | _scene.TaintedObject(delegate() | ||
403 | { | ||
404 | BulletSimAPI.SetObjectVelocity(_scene.WorldID, LocalID, _velocity); | ||
405 | }); | ||
406 | } | ||
407 | } | ||
408 | public override OMV.Vector3 Torque { | ||
409 | get { return _torque; } | ||
410 | set { _torque = value; | ||
411 | } | ||
412 | } | ||
413 | public override float CollisionScore { | ||
414 | get { return _collisionScore; } | ||
415 | set { _collisionScore = value; | ||
416 | } | ||
417 | } | ||
418 | public override OMV.Vector3 Acceleration { | ||
419 | get { return _acceleration; } | ||
420 | } | ||
421 | public override OMV.Quaternion Orientation { | ||
422 | get { return _orientation; } | ||
423 | set { | ||
424 | _orientation = value; | ||
425 | // m_log.DebugFormat("{0}: set orientation: id={1}, ori={2}", LogHeader, LocalID, _orientation); | ||
426 | _scene.TaintedObject(delegate() | ||
427 | { | ||
428 | // _position = BulletSimAPI.GetObjectPosition(_scene.WorldID, _localID); | ||
429 | BulletSimAPI.SetObjectTranslation(_scene.WorldID, _localID, _position, _orientation); | ||
430 | }); | ||
431 | } | ||
432 | } | ||
433 | public override int PhysicsActorType { | ||
434 | get { return _physicsActorType; } | ||
435 | set { _physicsActorType = value; | ||
436 | } | ||
437 | } | ||
438 | public override bool IsPhysical { | ||
439 | get { return _isPhysical; } | ||
440 | set { | ||
441 | _isPhysical = value; | ||
442 | _scene.TaintedObject(delegate() | ||
443 | { | ||
444 | SetObjectDynamic(); | ||
445 | }); | ||
446 | } | ||
447 | } | ||
448 | |||
449 | // An object is static (does not move) if selected or not physical | ||
450 | private bool IsStatic | ||
451 | { | ||
452 | get { return _isSelected || !IsPhysical; } | ||
453 | } | ||
454 | |||
455 | // An object is solid if it's not phantom and if it's not doing VolumeDetect | ||
456 | private bool IsSolid | ||
457 | { | ||
458 | get { return !IsPhantom && !_isVolumeDetect; } | ||
459 | } | ||
460 | |||
461 | // make gravity work if the object is physical and not selected | ||
462 | // no locking here because only called when it is safe | ||
463 | private void SetObjectDynamic() | ||
464 | { | ||
465 | // non-physical things work best with a mass of zero | ||
466 | _mass = IsStatic ? 0f : CalculateMass(); | ||
467 | BulletSimAPI.SetObjectProperties(_scene.WorldID, LocalID, IsStatic, IsSolid, SubscribedEvents(), _mass); | ||
468 | // m_log.DebugFormat("{0}: ID={1}, SetObjectDynamic: IsStatic={2}, IsSolid={3}, mass={4}", LogHeader, _localID, IsStatic, IsSolid, _mass); | ||
469 | } | ||
470 | |||
471 | // prims don't fly | ||
472 | public override bool Flying { | ||
473 | get { return _flying; } | ||
474 | set { _flying = value; } | ||
475 | } | ||
476 | public override bool SetAlwaysRun { | ||
477 | get { return _setAlwaysRun; } | ||
478 | set { _setAlwaysRun = value; } | ||
479 | } | ||
480 | public override bool ThrottleUpdates { | ||
481 | get { return _throttleUpdates; } | ||
482 | set { _throttleUpdates = value; } | ||
483 | } | ||
484 | public override bool IsColliding { | ||
485 | get { return (_collidingStep == _scene.SimulationStep); } | ||
486 | set { _isColliding = value; } | ||
487 | } | ||
488 | public override bool CollidingGround { | ||
489 | get { return (_collidingGroundStep == _scene.SimulationStep); } | ||
490 | set { _collidingGround = value; } | ||
491 | } | ||
492 | public override bool CollidingObj { | ||
493 | get { return _collidingObj; } | ||
494 | set { _collidingObj = value; } | ||
495 | } | ||
496 | public bool IsPhantom { | ||
497 | get { | ||
498 | // SceneObjectPart removes phantom objects from the physics scene | ||
499 | // so, although we could implement touching and such, we never | ||
500 | // are invoked as a phantom object | ||
501 | return false; | ||
502 | } | ||
503 | } | ||
504 | public override bool FloatOnWater { | ||
505 | set { _floatOnWater = value; } | ||
506 | } | ||
507 | public override OMV.Vector3 RotationalVelocity { | ||
508 | get { return _rotationalVelocity; } | ||
509 | set { _rotationalVelocity = value; | ||
510 | // m_log.DebugFormat("{0}: RotationalVelocity={1}", LogHeader, _rotationalVelocity); | ||
511 | _scene.TaintedObject(delegate() | ||
512 | { | ||
513 | BulletSimAPI.SetObjectAngularVelocity(_scene.WorldID, LocalID, _rotationalVelocity); | ||
514 | }); | ||
515 | } | ||
516 | } | ||
517 | public OMV.Vector3 AngularVelocity { | ||
518 | get { return _angularVelocity; } | ||
519 | set { _angularVelocity = value; } | ||
520 | } | ||
521 | public override bool Kinematic { | ||
522 | get { return _kinematic; } | ||
523 | set { _kinematic = value; | ||
524 | // m_log.DebugFormat("{0}: Kinematic={1}", LogHeader, _kinematic); | ||
525 | } | ||
526 | } | ||
527 | public override float Buoyancy { | ||
528 | get { return _buoyancy; } | ||
529 | set { _buoyancy = value; | ||
530 | _scene.TaintedObject(delegate() | ||
531 | { | ||
532 | BulletSimAPI.SetObjectBuoyancy(_scene.WorldID, _localID, _buoyancy); | ||
533 | }); | ||
534 | } | ||
535 | } | ||
536 | |||
537 | // Used for MoveTo | ||
538 | public override OMV.Vector3 PIDTarget { | ||
539 | set { _PIDTarget = value; } | ||
540 | } | ||
541 | public override bool PIDActive { | ||
542 | set { _usePID = value; } | ||
543 | } | ||
544 | public override float PIDTau { | ||
545 | set { _PIDTau = value; } | ||
546 | } | ||
547 | |||
548 | // Used for llSetHoverHeight and maybe vehicle height | ||
549 | // Hover Height will override MoveTo target's Z | ||
550 | public override bool PIDHoverActive { | ||
551 | set { _useHoverPID = value; } | ||
552 | } | ||
553 | public override float PIDHoverHeight { | ||
554 | set { _PIDHoverHeight = value; } | ||
555 | } | ||
556 | public override PIDHoverType PIDHoverType { | ||
557 | set { _PIDHoverType = value; } | ||
558 | } | ||
559 | public override float PIDHoverTau { | ||
560 | set { _PIDHoverTao = value; } | ||
561 | } | ||
562 | |||
563 | // For RotLookAt | ||
564 | public override OMV.Quaternion APIDTarget { set { return; } } | ||
565 | public override bool APIDActive { set { return; } } | ||
566 | public override float APIDStrength { set { return; } } | ||
567 | public override float APIDDamping { set { return; } } | ||
568 | |||
569 | public override void AddForce(OMV.Vector3 force, bool pushforce) { | ||
570 | if (force.IsFinite()) | ||
571 | { | ||
572 | _force.X += force.X; | ||
573 | _force.Y += force.Y; | ||
574 | _force.Z += force.Z; | ||
575 | } | ||
576 | else | ||
577 | { | ||
578 | m_log.WarnFormat("{0}: Got a NaN force applied to a Character", LogHeader); | ||
579 | } | ||
580 | _scene.TaintedObject(delegate() | ||
581 | { | ||
582 | BulletSimAPI.SetObjectForce(_scene.WorldID, _localID, _force); | ||
583 | }); | ||
584 | } | ||
585 | |||
586 | public override void AddAngularForce(OMV.Vector3 force, bool pushforce) { | ||
587 | // m_log.DebugFormat("{0}: AddAngularForce. f={1}, push={2}", LogHeader, force, pushforce); | ||
588 | } | ||
589 | public override void SetMomentum(OMV.Vector3 momentum) { | ||
590 | } | ||
591 | public override void SubscribeEvents(int ms) { | ||
592 | _subscribedEventsMs = ms; | ||
593 | _lastCollisionTime = Util.EnvironmentTickCount() - _subscribedEventsMs; // make first collision happen | ||
594 | } | ||
595 | public override void UnSubscribeEvents() { | ||
596 | _subscribedEventsMs = 0; | ||
597 | } | ||
598 | public override bool SubscribedEvents() { | ||
599 | return (_subscribedEventsMs > 0); | ||
600 | } | ||
601 | |||
602 | #region Mass Calculation | ||
603 | |||
604 | private float CalculateMass() | ||
605 | { | ||
606 | float volume = _size.X * _size.Y * _size.Z; // default | ||
607 | float tmp; | ||
608 | |||
609 | float returnMass = 0; | ||
610 | float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f; | ||
611 | float hollowVolume = hollowAmount * hollowAmount; | ||
612 | |||
613 | switch (_pbs.ProfileShape) | ||
614 | { | ||
615 | case ProfileShape.Square: | ||
616 | // default box | ||
617 | |||
618 | if (_pbs.PathCurve == (byte)Extrusion.Straight) | ||
619 | { | ||
620 | if (hollowAmount > 0.0) | ||
621 | { | ||
622 | switch (_pbs.HollowShape) | ||
623 | { | ||
624 | case HollowShape.Square: | ||
625 | case HollowShape.Same: | ||
626 | break; | ||
627 | |||
628 | case HollowShape.Circle: | ||
629 | |||
630 | hollowVolume *= 0.78539816339f; | ||
631 | break; | ||
632 | |||
633 | case HollowShape.Triangle: | ||
634 | |||
635 | hollowVolume *= (0.5f * .5f); | ||
636 | break; | ||
637 | |||
638 | default: | ||
639 | hollowVolume = 0; | ||
640 | break; | ||
641 | } | ||
642 | volume *= (1.0f - hollowVolume); | ||
643 | } | ||
644 | } | ||
645 | |||
646 | else if (_pbs.PathCurve == (byte)Extrusion.Curve1) | ||
647 | { | ||
648 | //a tube | ||
649 | |||
650 | volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX); | ||
651 | tmp= 1.0f -2.0e-2f * (float)(200 - _pbs.PathScaleY); | ||
652 | volume -= volume*tmp*tmp; | ||
653 | |||
654 | if (hollowAmount > 0.0) | ||
655 | { | ||
656 | hollowVolume *= hollowAmount; | ||
657 | |||
658 | switch (_pbs.HollowShape) | ||
659 | { | ||
660 | case HollowShape.Square: | ||
661 | case HollowShape.Same: | ||
662 | break; | ||
663 | |||
664 | case HollowShape.Circle: | ||
665 | hollowVolume *= 0.78539816339f;; | ||
666 | break; | ||
667 | |||
668 | case HollowShape.Triangle: | ||
669 | hollowVolume *= 0.5f * 0.5f; | ||
670 | break; | ||
671 | default: | ||
672 | hollowVolume = 0; | ||
673 | break; | ||
674 | } | ||
675 | volume *= (1.0f - hollowVolume); | ||
676 | } | ||
677 | } | ||
678 | |||
679 | break; | ||
680 | |||
681 | case ProfileShape.Circle: | ||
682 | |||
683 | if (_pbs.PathCurve == (byte)Extrusion.Straight) | ||
684 | { | ||
685 | volume *= 0.78539816339f; // elipse base | ||
686 | |||
687 | if (hollowAmount > 0.0) | ||
688 | { | ||
689 | switch (_pbs.HollowShape) | ||
690 | { | ||
691 | case HollowShape.Same: | ||
692 | case HollowShape.Circle: | ||
693 | break; | ||
694 | |||
695 | case HollowShape.Square: | ||
696 | hollowVolume *= 0.5f * 2.5984480504799f; | ||
697 | break; | ||
698 | |||
699 | case HollowShape.Triangle: | ||
700 | hollowVolume *= .5f * 1.27323954473516f; | ||
701 | break; | ||
702 | |||
703 | default: | ||
704 | hollowVolume = 0; | ||
705 | break; | ||
706 | } | ||
707 | volume *= (1.0f - hollowVolume); | ||
708 | } | ||
709 | } | ||
710 | |||
711 | else if (_pbs.PathCurve == (byte)Extrusion.Curve1) | ||
712 | { | ||
713 | volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX); | ||
714 | tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); | ||
715 | volume *= (1.0f - tmp * tmp); | ||
716 | |||
717 | if (hollowAmount > 0.0) | ||
718 | { | ||
719 | |||
720 | // calculate the hollow volume by it's shape compared to the prim shape | ||
721 | hollowVolume *= hollowAmount; | ||
722 | |||
723 | switch (_pbs.HollowShape) | ||
724 | { | ||
725 | case HollowShape.Same: | ||
726 | case HollowShape.Circle: | ||
727 | break; | ||
728 | |||
729 | case HollowShape.Square: | ||
730 | hollowVolume *= 0.5f * 2.5984480504799f; | ||
731 | break; | ||
732 | |||
733 | case HollowShape.Triangle: | ||
734 | hollowVolume *= .5f * 1.27323954473516f; | ||
735 | break; | ||
736 | |||
737 | default: | ||
738 | hollowVolume = 0; | ||
739 | break; | ||
740 | } | ||
741 | volume *= (1.0f - hollowVolume); | ||
742 | } | ||
743 | } | ||
744 | break; | ||
745 | |||
746 | case ProfileShape.HalfCircle: | ||
747 | if (_pbs.PathCurve == (byte)Extrusion.Curve1) | ||
748 | { | ||
749 | volume *= 0.52359877559829887307710723054658f; | ||
750 | } | ||
751 | break; | ||
752 | |||
753 | case ProfileShape.EquilateralTriangle: | ||
754 | |||
755 | if (_pbs.PathCurve == (byte)Extrusion.Straight) | ||
756 | { | ||
757 | volume *= 0.32475953f; | ||
758 | |||
759 | if (hollowAmount > 0.0) | ||
760 | { | ||
761 | |||
762 | // calculate the hollow volume by it's shape compared to the prim shape | ||
763 | switch (_pbs.HollowShape) | ||
764 | { | ||
765 | case HollowShape.Same: | ||
766 | case HollowShape.Triangle: | ||
767 | hollowVolume *= .25f; | ||
768 | break; | ||
769 | |||
770 | case HollowShape.Square: | ||
771 | hollowVolume *= 0.499849f * 3.07920140172638f; | ||
772 | break; | ||
773 | |||
774 | case HollowShape.Circle: | ||
775 | // Hollow shape is a perfect cyllinder in respect to the cube's scale | ||
776 | // Cyllinder hollow volume calculation | ||
777 | |||
778 | hollowVolume *= 0.1963495f * 3.07920140172638f; | ||
779 | break; | ||
780 | |||
781 | default: | ||
782 | hollowVolume = 0; | ||
783 | break; | ||
784 | } | ||
785 | volume *= (1.0f - hollowVolume); | ||
786 | } | ||
787 | } | ||
788 | else if (_pbs.PathCurve == (byte)Extrusion.Curve1) | ||
789 | { | ||
790 | volume *= 0.32475953f; | ||
791 | volume *= 0.01f * (float)(200 - _pbs.PathScaleX); | ||
792 | tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY); | ||
793 | volume *= (1.0f - tmp * tmp); | ||
794 | |||
795 | if (hollowAmount > 0.0) | ||
796 | { | ||
797 | |||
798 | hollowVolume *= hollowAmount; | ||
799 | |||
800 | switch (_pbs.HollowShape) | ||
801 | { | ||
802 | case HollowShape.Same: | ||
803 | case HollowShape.Triangle: | ||
804 | hollowVolume *= .25f; | ||
805 | break; | ||
806 | |||
807 | case HollowShape.Square: | ||
808 | hollowVolume *= 0.499849f * 3.07920140172638f; | ||
809 | break; | ||
810 | |||
811 | case HollowShape.Circle: | ||
812 | |||
813 | hollowVolume *= 0.1963495f * 3.07920140172638f; | ||
814 | break; | ||
815 | |||
816 | default: | ||
817 | hollowVolume = 0; | ||
818 | break; | ||
819 | } | ||
820 | volume *= (1.0f - hollowVolume); | ||
821 | } | ||
822 | } | ||
823 | break; | ||
824 | |||
825 | default: | ||
826 | break; | ||
827 | } | ||
828 | |||
829 | |||
830 | |||
831 | float taperX1; | ||
832 | float taperY1; | ||
833 | float taperX; | ||
834 | float taperY; | ||
835 | float pathBegin; | ||
836 | float pathEnd; | ||
837 | float profileBegin; | ||
838 | float profileEnd; | ||
839 | |||
840 | if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible) | ||
841 | { | ||
842 | taperX1 = _pbs.PathScaleX * 0.01f; | ||
843 | if (taperX1 > 1.0f) | ||
844 | taperX1 = 2.0f - taperX1; | ||
845 | taperX = 1.0f - taperX1; | ||
846 | |||
847 | taperY1 = _pbs.PathScaleY * 0.01f; | ||
848 | if (taperY1 > 1.0f) | ||
849 | taperY1 = 2.0f - taperY1; | ||
850 | taperY = 1.0f - taperY1; | ||
851 | } | ||
852 | else | ||
853 | { | ||
854 | taperX = _pbs.PathTaperX * 0.01f; | ||
855 | if (taperX < 0.0f) | ||
856 | taperX = -taperX; | ||
857 | taperX1 = 1.0f - taperX; | ||
858 | |||
859 | taperY = _pbs.PathTaperY * 0.01f; | ||
860 | if (taperY < 0.0f) | ||
861 | taperY = -taperY; | ||
862 | taperY1 = 1.0f - taperY; | ||
863 | |||
864 | } | ||
865 | |||
866 | |||
867 | volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY); | ||
868 | |||
869 | pathBegin = (float)_pbs.PathBegin * 2.0e-5f; | ||
870 | pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f; | ||
871 | volume *= (pathEnd - pathBegin); | ||
872 | |||
873 | // this is crude aproximation | ||
874 | profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f; | ||
875 | profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f; | ||
876 | volume *= (profileEnd - profileBegin); | ||
877 | |||
878 | returnMass = _density * volume; | ||
879 | |||
880 | if (IsRootOfLinkset) | ||
881 | { | ||
882 | foreach (BSPrim prim in _childrenPrims) | ||
883 | { | ||
884 | returnMass += prim.CalculateMass(); | ||
885 | } | ||
886 | } | ||
887 | |||
888 | if (returnMass <= 0) | ||
889 | returnMass = 0.0001f; | ||
890 | |||
891 | if (returnMass > _scene.MaximumObjectMass) | ||
892 | returnMass = _scene.MaximumObjectMass; | ||
893 | |||
894 | return returnMass; | ||
895 | }// end CalculateMass | ||
896 | #endregion Mass Calculation | ||
897 | |||
898 | // Create the geometry information in Bullet for later use | ||
899 | // No locking here because this is done when we know physics is not simulating | ||
900 | private void CreateGeom() | ||
901 | { | ||
902 | // Since we're recreating new, get rid of any previously generated shape | ||
903 | if (_hullKey != 0) | ||
904 | { | ||
905 | // m_log.DebugFormat("{0}: CreateGeom: deleting old hull. Key={1}", LogHeader, _hullKey); | ||
906 | BulletSimAPI.DestroyHull(_scene.WorldID, _hullKey); | ||
907 | _hullKey = 0; | ||
908 | _hulls.Clear(); | ||
909 | } | ||
910 | |||
911 | if (_mesh == null) | ||
912 | { | ||
913 | // the mesher thought this was too simple to mesh. Use a native Bullet collision shape. | ||
914 | if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1) | ||
915 | { | ||
916 | if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z) | ||
917 | { | ||
918 | // m_log.DebugFormat("{0}: CreateGeom: mesh null. Defaulting to sphere of size {1}", LogHeader, _size); | ||
919 | _shapeType = ShapeData.PhysicsShapeType.SHAPE_SPHERE; | ||
920 | // Bullet native objects are scaled by the Bullet engine so pass the size in | ||
921 | _scale = _size; | ||
922 | } | ||
923 | } | ||
924 | else | ||
925 | { | ||
926 | // m_log.DebugFormat("{0}: CreateGeom: mesh null. Defaulting to box. lid={1}, size={2}", LogHeader, LocalID, _size); | ||
927 | _shapeType = ShapeData.PhysicsShapeType.SHAPE_BOX; | ||
928 | _scale = _size; | ||
929 | } | ||
930 | } | ||
931 | else | ||
932 | { | ||
933 | int[] indices = _mesh.getIndexListAsInt(); | ||
934 | List<OMV.Vector3> vertices = _mesh.getVertexList(); | ||
935 | |||
936 | //format conversion from IMesh format to DecompDesc format | ||
937 | List<int> convIndices = new List<int>(); | ||
938 | List<float3> convVertices = new List<float3>(); | ||
939 | for (int ii = 0; ii < indices.GetLength(0); ii++) | ||
940 | { | ||
941 | convIndices.Add(indices[ii]); | ||
942 | } | ||
943 | foreach (OMV.Vector3 vv in vertices) | ||
944 | { | ||
945 | convVertices.Add(new float3(vv.X, vv.Y, vv.Z)); | ||
946 | } | ||
947 | |||
948 | // setup and do convex hull conversion | ||
949 | _hulls = new List<ConvexResult>(); | ||
950 | DecompDesc dcomp = new DecompDesc(); | ||
951 | dcomp.mIndices = convIndices; | ||
952 | dcomp.mVertices = convVertices; | ||
953 | ConvexBuilder convexBuilder = new ConvexBuilder(HullReturn); | ||
954 | // create the hull into the _hulls variable | ||
955 | convexBuilder.process(dcomp); | ||
956 | |||
957 | // Convert the vertices and indices for passing to unmanaged | ||
958 | // The hull information is passed as a large floating point array. | ||
959 | // The format is: | ||
960 | // convHulls[0] = number of hulls | ||
961 | // convHulls[1] = number of vertices in first hull | ||
962 | // convHulls[2] = hull centroid X coordinate | ||
963 | // convHulls[3] = hull centroid Y coordinate | ||
964 | // convHulls[4] = hull centroid Z coordinate | ||
965 | // convHulls[5] = first hull vertex X | ||
966 | // convHulls[6] = first hull vertex Y | ||
967 | // convHulls[7] = first hull vertex Z | ||
968 | // convHulls[8] = second hull vertex X | ||
969 | // ... | ||
970 | // convHulls[n] = number of vertices in second hull | ||
971 | // convHulls[n+1] = second hull centroid X coordinate | ||
972 | // ... | ||
973 | // | ||
974 | // TODO: is is very inefficient. Someday change the convex hull generator to return | ||
975 | // data structures that do not need to be converted in order to pass to Bullet. | ||
976 | // And maybe put the values directly into pinned memory rather than marshaling. | ||
977 | int hullCount = _hulls.Count; | ||
978 | int totalVertices = 1; // include one for the count of the hulls | ||
979 | foreach (ConvexResult cr in _hulls) | ||
980 | { | ||
981 | totalVertices += 4; // add four for the vertex count and centroid | ||
982 | totalVertices += cr.HullIndices.Count * 3; // we pass just triangles | ||
983 | } | ||
984 | float[] convHulls = new float[totalVertices]; | ||
985 | |||
986 | convHulls[0] = (float)hullCount; | ||
987 | int jj = 1; | ||
988 | foreach (ConvexResult cr in _hulls) | ||
989 | { | ||
990 | // copy vertices for index access | ||
991 | float3[] verts = new float3[cr.HullVertices.Count]; | ||
992 | int kk = 0; | ||
993 | foreach (float3 ff in cr.HullVertices) | ||
994 | { | ||
995 | verts[kk++] = ff; | ||
996 | } | ||
997 | |||
998 | // add to the array one hull's worth of data | ||
999 | convHulls[jj++] = cr.HullIndices.Count; | ||
1000 | convHulls[jj++] = 0f; // centroid x,y,z | ||
1001 | convHulls[jj++] = 0f; | ||
1002 | convHulls[jj++] = 0f; | ||
1003 | foreach (int ind in cr.HullIndices) | ||
1004 | { | ||
1005 | convHulls[jj++] = verts[ind].x; | ||
1006 | convHulls[jj++] = verts[ind].y; | ||
1007 | convHulls[jj++] = verts[ind].z; | ||
1008 | } | ||
1009 | } | ||
1010 | |||
1011 | // create the hull definition in Bullet | ||
1012 | _hullKey = (ulong)_pbs.GetHashCode(); | ||
1013 | // m_log.DebugFormat("{0}: CreateGeom: calling CreateHull. lid={1}, key={2}, hulls={3}", LogHeader, _localID, _hullKey, hullCount); | ||
1014 | BulletSimAPI.CreateHull(_scene.WorldID, _hullKey, hullCount, convHulls); | ||
1015 | _shapeType = ShapeData.PhysicsShapeType.SHAPE_HULL; | ||
1016 | // meshes are already scaled by the meshmerizer | ||
1017 | _scale = new OMV.Vector3(1f, 1f, 1f); | ||
1018 | } | ||
1019 | return; | ||
1020 | } | ||
1021 | |||
1022 | // Callback from convex hull creater with a newly created hull. | ||
1023 | // Just add it to the collection of hulls for this shape. | ||
1024 | private void HullReturn(ConvexResult result) | ||
1025 | { | ||
1026 | _hulls.Add(result); | ||
1027 | return; | ||
1028 | } | ||
1029 | |||
1030 | // Create an object in Bullet | ||
1031 | // No locking here because this is done when the physics engine is not simulating | ||
1032 | private void CreateObject() | ||
1033 | { | ||
1034 | if (IsRootOfLinkset) | ||
1035 | { | ||
1036 | // Create a linkset around this object | ||
1037 | // CreateLinksetWithCompoundHull(); | ||
1038 | CreateLinksetWithConstraints(); | ||
1039 | } | ||
1040 | else | ||
1041 | { | ||
1042 | // simple object | ||
1043 | ShapeData shape; | ||
1044 | FillShapeInfo(out shape); | ||
1045 | BulletSimAPI.CreateObject(_scene.WorldID, shape); | ||
1046 | } | ||
1047 | } | ||
1048 | |||
1049 | // Create a linkset by creating a compound hull at the root prim that consists of all | ||
1050 | // the children. | ||
1051 | // NOTE: This does not allow proper collisions with the children prims so it is not a workable solution | ||
1052 | void CreateLinksetWithCompoundHull() | ||
1053 | { | ||
1054 | // If I am the root prim of a linkset, replace my physical shape with all the | ||
1055 | // pieces of the children. | ||
1056 | // All of the children should have called CreateGeom so they have a hull | ||
1057 | // in the physics engine already. Here we pull together all of those hulls | ||
1058 | // into one shape. | ||
1059 | int totalPrimsInLinkset = _childrenPrims.Count + 1; | ||
1060 | // m_log.DebugFormat("{0}: CreateLinkset. Root prim={1}, prims={2}", LogHeader, LocalID, totalPrimsInLinkset); | ||
1061 | ShapeData[] shapes = new ShapeData[totalPrimsInLinkset]; | ||
1062 | FillShapeInfo(out shapes[0]); | ||
1063 | int ii = 1; | ||
1064 | foreach (BSPrim prim in _childrenPrims) | ||
1065 | { | ||
1066 | // m_log.DebugFormat("{0}: CreateLinkset: adding prim {1}", LogHeader, prim.LocalID); | ||
1067 | prim.FillShapeInfo(out shapes[ii]); | ||
1068 | ii++; | ||
1069 | } | ||
1070 | BulletSimAPI.CreateLinkset(_scene.WorldID, totalPrimsInLinkset, shapes); | ||
1071 | } | ||
1072 | |||
1073 | // Copy prim's info into the BulletSim shape description structure | ||
1074 | public void FillShapeInfo(out ShapeData shape) | ||
1075 | { | ||
1076 | shape.ID = _localID; | ||
1077 | shape.Type = _shapeType; | ||
1078 | shape.Position = _position; | ||
1079 | shape.Rotation = _orientation; | ||
1080 | shape.Velocity = _velocity; | ||
1081 | shape.Scale = _scale; | ||
1082 | shape.Mass = _isPhysical ? _mass : 0f; | ||
1083 | shape.Buoyancy = _buoyancy; | ||
1084 | shape.MeshKey = _hullKey; | ||
1085 | shape.Friction = _friction; | ||
1086 | shape.Restitution = _restitution; | ||
1087 | shape.Collidable = (!IsPhantom) ? ShapeData.numericTrue : ShapeData.numericFalse; | ||
1088 | shape.Static = _isPhysical ? ShapeData.numericFalse : ShapeData.numericTrue; | ||
1089 | } | ||
1090 | |||
1091 | // Create the linkset by putting constraints between the objects of the set so they cannot move | ||
1092 | // relative to each other. | ||
1093 | // TODO: make this more effeicient: a large linkset gets rebuilt over and over and prims are added | ||
1094 | void CreateLinksetWithConstraints() | ||
1095 | { | ||
1096 | // m_log.DebugFormat("{0}: CreateLinkset. Root prim={1}, prims={2}", LogHeader, LocalID, _childrenPrims.Count+1); | ||
1097 | |||
1098 | // remove any constraints that might be in place | ||
1099 | foreach (BSPrim prim in _childrenPrims) | ||
1100 | { | ||
1101 | // m_log.DebugFormat("{0}: CreateObject: RemoveConstraint between root prim {1} and child prim {2}", LogHeader, LocalID, prim.LocalID); | ||
1102 | BulletSimAPI.RemoveConstraint(_scene.WorldID, LocalID, prim.LocalID); | ||
1103 | } | ||
1104 | // create constraints between the root prim and each of the children | ||
1105 | foreach (BSPrim prim in _childrenPrims) | ||
1106 | { | ||
1107 | // m_log.DebugFormat("{0}: CreateObject: AddConstraint between root prim {1} and child prim {2}", LogHeader, LocalID, prim.LocalID); | ||
1108 | |||
1109 | // Zero motion for children so they don't interpolate | ||
1110 | prim.ZeroMotion(); | ||
1111 | |||
1112 | // relative position normalized to the root prim | ||
1113 | OMV.Vector3 childRelativePosition = (prim._position - this._position) * OMV.Quaternion.Inverse(this._orientation); | ||
1114 | |||
1115 | // relative rotation of the child to the parent | ||
1116 | OMV.Quaternion relativeRotation = OMV.Quaternion.Inverse(prim._orientation) * this._orientation; | ||
1117 | |||
1118 | // this is a constraint that allows no freedom of movement between the two objects | ||
1119 | // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818 | ||
1120 | BulletSimAPI.AddConstraint(_scene.WorldID, LocalID, prim.LocalID, | ||
1121 | childRelativePosition, | ||
1122 | relativeRotation, | ||
1123 | OMV.Vector3.Zero, | ||
1124 | OMV.Quaternion.Identity, | ||
1125 | OMV.Vector3.Zero, OMV.Vector3.Zero, | ||
1126 | OMV.Vector3.Zero, OMV.Vector3.Zero); | ||
1127 | } | ||
1128 | } | ||
1129 | |||
1130 | // Rebuild the geometry and object. | ||
1131 | // This is called when the shape changes so we need to recreate the mesh/hull. | ||
1132 | // No locking here because this is done when the physics engine is not simulating | ||
1133 | private void RecreateGeomAndObject() | ||
1134 | { | ||
1135 | // If this object is complex or we are the root of a linkset, build a mesh. | ||
1136 | // The root of a linkset must be a mesh so we can create the linked compound object. | ||
1137 | // if (_scene.NeedsMeshing(_pbs) || IsRootOfLinkset ) | ||
1138 | if (_scene.NeedsMeshing(_pbs)) // linksets with constraints don't need a root mesh | ||
1139 | { | ||
1140 | // m_log.DebugFormat("{0}: RecreateGeomAndObject: creating mesh", LogHeader); | ||
1141 | _mesh = _scene.mesher.CreateMesh(_avName, _pbs, _size, _scene.MeshLOD, _isPhysical); | ||
1142 | } | ||
1143 | else | ||
1144 | { | ||
1145 | // implement the shape with a Bullet native shape. | ||
1146 | _mesh = null; | ||
1147 | } | ||
1148 | CreateGeom(); | ||
1149 | CreateObject(); | ||
1150 | return; | ||
1151 | } | ||
1152 | |||
1153 | // The physics engine says that properties have updated. Update same and inform | ||
1154 | // the world that things have changed. | ||
1155 | // TODO: do we really need to check for changed? Maybe just copy values and call RequestPhysicsterseUpdate() | ||
1156 | enum UpdatedProperties { | ||
1157 | Position = 1 << 0, | ||
1158 | Rotation = 1 << 1, | ||
1159 | Velocity = 1 << 2, | ||
1160 | Acceleration = 1 << 3, | ||
1161 | RotationalVel = 1 << 4 | ||
1162 | } | ||
1163 | |||
1164 | const float ROTATION_TOLERANCE = 0.01f; | ||
1165 | const float VELOCITY_TOLERANCE = 0.001f; | ||
1166 | const float POSITION_TOLERANCE = 0.05f; | ||
1167 | const float ACCELERATION_TOLERANCE = 0.01f; | ||
1168 | const float ROTATIONAL_VELOCITY_TOLERANCE = 0.01f; | ||
1169 | const bool SHOULD_DAMP_UPDATES = false; | ||
1170 | |||
1171 | public void UpdateProperties(EntityProperties entprop) | ||
1172 | { | ||
1173 | UpdatedProperties changed = 0; | ||
1174 | if (SHOULD_DAMP_UPDATES) | ||
1175 | { | ||
1176 | // assign to the local variables so the normal set action does not happen | ||
1177 | // if (_position != entprop.Position) | ||
1178 | if (!_position.ApproxEquals(entprop.Position, POSITION_TOLERANCE)) | ||
1179 | { | ||
1180 | _position = entprop.Position; | ||
1181 | // m_log.DebugFormat("{0}: UpdateProperties: id={1}, pos = {2}", LogHeader, LocalID, _position); | ||
1182 | changed |= UpdatedProperties.Position; | ||
1183 | } | ||
1184 | // if (_orientation != entprop.Rotation) | ||
1185 | if (!_orientation.ApproxEquals(entprop.Rotation, ROTATION_TOLERANCE)) | ||
1186 | { | ||
1187 | _orientation = entprop.Rotation; | ||
1188 | // m_log.DebugFormat("{0}: UpdateProperties: id={1}, rot = {2}", LogHeader, LocalID, _orientation); | ||
1189 | changed |= UpdatedProperties.Rotation; | ||
1190 | } | ||
1191 | // if (_velocity != entprop.Velocity) | ||
1192 | if (!_velocity.ApproxEquals(entprop.Velocity, VELOCITY_TOLERANCE)) | ||
1193 | { | ||
1194 | _velocity = entprop.Velocity; | ||
1195 | // m_log.DebugFormat("{0}: UpdateProperties: velocity = {1}", LogHeader, _velocity); | ||
1196 | changed |= UpdatedProperties.Velocity; | ||
1197 | } | ||
1198 | // if (_acceleration != entprop.Acceleration) | ||
1199 | if (!_acceleration.ApproxEquals(entprop.Acceleration, ACCELERATION_TOLERANCE)) | ||
1200 | { | ||
1201 | _acceleration = entprop.Acceleration; | ||
1202 | // m_log.DebugFormat("{0}: UpdateProperties: acceleration = {1}", LogHeader, _acceleration); | ||
1203 | changed |= UpdatedProperties.Acceleration; | ||
1204 | } | ||
1205 | // if (_rotationalVelocity != entprop.RotationalVelocity) | ||
1206 | if (!_rotationalVelocity.ApproxEquals(entprop.RotationalVelocity, ROTATIONAL_VELOCITY_TOLERANCE)) | ||
1207 | { | ||
1208 | _rotationalVelocity = entprop.RotationalVelocity; | ||
1209 | // m_log.DebugFormat("{0}: UpdateProperties: rotationalVelocity = {1}", LogHeader, _rotationalVelocity); | ||
1210 | changed |= UpdatedProperties.RotationalVel; | ||
1211 | } | ||
1212 | if (changed != 0) | ||
1213 | { | ||
1214 | // m_log.DebugFormat("{0}: UpdateProperties: id={1}, c={2}, pos={3}, rot={4}", LogHeader, LocalID, changed, _position, _orientation); | ||
1215 | // Only update the position of single objects and linkset roots | ||
1216 | if (this._parentPrim == null) | ||
1217 | { | ||
1218 | // m_log.DebugFormat("{0}: RequestTerseUpdate. id={1}, ch={2}, pos={3}, rot={4}", LogHeader, LocalID, changed, _position, _orientation); | ||
1219 | base.RequestPhysicsterseUpdate(); | ||
1220 | } | ||
1221 | } | ||
1222 | } | ||
1223 | else | ||
1224 | { | ||
1225 | // Don't check for damping here -- it's done in BulletSim and SceneObjectPart. | ||
1226 | if (this._parentPrim == null) | ||
1227 | { | ||
1228 | // Assign to the local variables so the normal set action does not happen | ||
1229 | _position = entprop.Position; | ||
1230 | _orientation = entprop.Rotation; | ||
1231 | _velocity = entprop.Velocity; | ||
1232 | _acceleration = entprop.Acceleration; | ||
1233 | _rotationalVelocity = entprop.RotationalVelocity; | ||
1234 | // m_log.DebugFormat("{0}: RequestTerseUpdate. id={1}, ch={2}, pos={3}, rot={4}", LogHeader, LocalID, changed, _position, _orientation); | ||
1235 | base.RequestPhysicsterseUpdate(); | ||
1236 | } | ||
1237 | } | ||
1238 | } | ||
1239 | |||
1240 | // I've collided with something | ||
1241 | public void Collide(uint collidingWith, ActorTypes type, OMV.Vector3 contactPoint, OMV.Vector3 contactNormal, float pentrationDepth) | ||
1242 | { | ||
1243 | // m_log.DebugFormat("{0}: Collide: ms={1}, id={2}, with={3}", LogHeader, _subscribedEventsMs, LocalID, collidingWith); | ||
1244 | |||
1245 | // The following lines make IsColliding() and IsCollidingGround() work | ||
1246 | _collidingStep = _scene.SimulationStep; | ||
1247 | if (collidingWith == BSScene.TERRAIN_ID || collidingWith == BSScene.GROUNDPLANE_ID) | ||
1248 | { | ||
1249 | _collidingGroundStep = _scene.SimulationStep; | ||
1250 | } | ||
1251 | |||
1252 | if (_subscribedEventsMs == 0) return; // nothing in the object is waiting for collision events | ||
1253 | // throttle the collisions to the number of milliseconds specified in the subscription | ||
1254 | int nowTime = _scene.SimulationNowTime; | ||
1255 | if (nowTime < (_lastCollisionTime + _subscribedEventsMs)) return; | ||
1256 | _lastCollisionTime = nowTime; | ||
1257 | |||
1258 | // create the event for the collision | ||
1259 | Dictionary<uint, ContactPoint> contactPoints = new Dictionary<uint, ContactPoint>(); | ||
1260 | contactPoints.Add(collidingWith, new ContactPoint(contactPoint, contactNormal, pentrationDepth)); | ||
1261 | CollisionEventUpdate args = new CollisionEventUpdate(LocalID, (int)type, 1, contactPoints); | ||
1262 | base.SendCollisionUpdate(args); | ||
1263 | } | ||
1264 | } | ||
1265 | } | ||
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs new file mode 100644 index 0000000..de86d59 --- /dev/null +++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs | |||
@@ -0,0 +1,655 @@ | |||
1 | /* | ||
2 | * Copyright (c) Contributors, http://opensimulator.org/ | ||
3 | * See CONTRIBUTORS.TXT for a full list of copyright holders. | ||
4 | * | ||
5 | * Redistribution and use in source and binary forms, with or without | ||
6 | * modification, are permitted provided that the following conditions are met: | ||
7 | * * Redistributions of source code must retain the above copyright | ||
8 | * notice, this list of conditions and the following disclaimer. | ||
9 | * * Redistributions in binary form must reproduce the above copyrightD | ||
10 | * notice, this list of conditions and the following disclaimer in the | ||
11 | * documentation and/or other materials provided with the distribution. | ||
12 | * * Neither the name of the OpenSimulator Project nor the | ||
13 | * names of its contributors may be used to endorse or promote products | ||
14 | * derived from this software without specific prior written permission. | ||
15 | * | ||
16 | * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY | ||
17 | * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED | ||
18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE | ||
19 | * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY | ||
20 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES | ||
21 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
22 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND | ||
23 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
24 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS | ||
25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. | ||
26 | */ | ||
27 | using System; | ||
28 | using System.Collections.Generic; | ||
29 | using System.Runtime.InteropServices; | ||
30 | using System.Text; | ||
31 | using System.Threading; | ||
32 | using Nini.Config; | ||
33 | using log4net; | ||
34 | using OpenSim.Framework; | ||
35 | using OpenSim.Region.Physics.Manager; | ||
36 | using OpenMetaverse; | ||
37 | using OpenSim.Region.Framework; | ||
38 | |||
39 | // TODOs for BulletSim (for BSScene, BSPrim, BSCharacter and BulletSim) | ||
40 | // Parameterize BulletSim. Pass a structure of parameters to the C++ code. Capsule size, friction, ... | ||
41 | // Adjust character capsule size when height is adjusted (ScenePresence.SetHeight) | ||
42 | // Test sculpties | ||
43 | // Compute physics FPS reasonably | ||
44 | // Based on material, set density and friction | ||
45 | // More efficient memory usage in passing hull information from BSPrim to BulletSim | ||
46 | // Four states of prim: Physical, regular, phantom and selected. Are we modeling these correctly? | ||
47 | // In SL one can set both physical and phantom (gravity, does not effect others, makes collisions with ground) | ||
48 | // At the moment, physical and phantom causes object to drop through the terrain | ||
49 | // Should prim.link() and prim.delink() membership checking happen at taint time? | ||
50 | // Mesh sharing. Use meshHash to tell if we already have a hull of that shape and only create once | ||
51 | // Do attachments need to be handled separately? Need collision events. Do not collide with VolumeDetect | ||
52 | // Implement the genCollisions feature in BulletSim::SetObjectProperties (don't pass up unneeded collisions) | ||
53 | // Implement LockAngularMotion | ||
54 | // Decide if clearing forces is the right thing to do when setting position (BulletSim::SetObjectTranslation) | ||
55 | // Built Galton board (lots of MoveTo's) and some slats were not positioned correctly (mistakes scattered) | ||
56 | // No mistakes with ODE. Shape creation race condition? | ||
57 | // Does NeedsMeshing() really need to exclude all the different shapes? | ||
58 | // | ||
59 | namespace OpenSim.Region.Physics.BulletSPlugin | ||
60 | { | ||
61 | public class BSScene : PhysicsScene | ||
62 | { | ||
63 | private static readonly ILog m_log = LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType); | ||
64 | private static readonly string LogHeader = "[BULLETS SCENE]"; | ||
65 | |||
66 | private Dictionary<uint, BSCharacter> m_avatars = new Dictionary<uint, BSCharacter>(); | ||
67 | private Dictionary<uint, BSPrim> m_prims = new Dictionary<uint, BSPrim>(); | ||
68 | private List<BSPrim> m_vehicles = new List<BSPrim>(); | ||
69 | private float[] m_heightMap; | ||
70 | private float m_waterLevel; | ||
71 | private uint m_worldID; | ||
72 | public uint WorldID { get { return m_worldID; } } | ||
73 | |||
74 | private bool m_initialized = false; | ||
75 | |||
76 | public IMesher mesher; | ||
77 | private int m_meshLOD; | ||
78 | public int MeshLOD | ||
79 | { | ||
80 | get { return m_meshLOD; } | ||
81 | } | ||
82 | |||
83 | private int m_maxSubSteps; | ||
84 | private float m_fixedTimeStep; | ||
85 | private long m_simulationStep = 0; | ||
86 | public long SimulationStep { get { return m_simulationStep; } } | ||
87 | |||
88 | // A value of the time now so all the collision and update routines do not have to get their own | ||
89 | // Set to 'now' just before all the prims and actors are called for collisions and updates | ||
90 | private int m_simulationNowTime; | ||
91 | public int SimulationNowTime { get { return m_simulationNowTime; } } | ||
92 | |||
93 | private int m_maxCollisionsPerFrame; | ||
94 | private CollisionDesc[] m_collisionArray; | ||
95 | private GCHandle m_collisionArrayPinnedHandle; | ||
96 | |||
97 | private int m_maxUpdatesPerFrame; | ||
98 | private EntityProperties[] m_updateArray; | ||
99 | private GCHandle m_updateArrayPinnedHandle; | ||
100 | |||
101 | private bool _meshSculptedPrim = true; // cause scuplted prims to get meshed | ||
102 | private bool _forceSimplePrimMeshing = false; // if a cube or sphere, let Bullet do internal shapes | ||
103 | |||
104 | public const uint TERRAIN_ID = 0; // OpenSim senses terrain with a localID of zero | ||
105 | public const uint GROUNDPLANE_ID = 1; | ||
106 | |||
107 | public ConfigurationParameters Params | ||
108 | { | ||
109 | get { return m_params[0]; } | ||
110 | } | ||
111 | public Vector3 DefaultGravity | ||
112 | { | ||
113 | get { return new Vector3(0f, 0f, Params.gravity); } | ||
114 | } | ||
115 | |||
116 | private float m_maximumObjectMass; | ||
117 | public float MaximumObjectMass | ||
118 | { | ||
119 | get { return m_maximumObjectMass; } | ||
120 | } | ||
121 | |||
122 | public delegate void TaintCallback(); | ||
123 | private List<TaintCallback> _taintedObjects; | ||
124 | private Object _taintLock = new Object(); | ||
125 | |||
126 | // A pointer to an instance if this structure is passed to the C++ code | ||
127 | ConfigurationParameters[] m_params; | ||
128 | GCHandle m_paramsHandle; | ||
129 | |||
130 | private BulletSimAPI.DebugLogCallback debugLogCallbackHandle; | ||
131 | |||
132 | public BSScene(string identifier) | ||
133 | { | ||
134 | m_initialized = false; | ||
135 | } | ||
136 | |||
137 | public override void Initialise(IMesher meshmerizer, IConfigSource config) | ||
138 | { | ||
139 | // Allocate pinned memory to pass parameters. | ||
140 | m_params = new ConfigurationParameters[1]; | ||
141 | m_paramsHandle = GCHandle.Alloc(m_params, GCHandleType.Pinned); | ||
142 | |||
143 | // Set default values for physics parameters plus any overrides from the ini file | ||
144 | GetInitialParameterValues(config); | ||
145 | |||
146 | // allocate more pinned memory close to the above in an attempt to get the memory all together | ||
147 | m_collisionArray = new CollisionDesc[m_maxCollisionsPerFrame]; | ||
148 | m_collisionArrayPinnedHandle = GCHandle.Alloc(m_collisionArray, GCHandleType.Pinned); | ||
149 | m_updateArray = new EntityProperties[m_maxUpdatesPerFrame]; | ||
150 | m_updateArrayPinnedHandle = GCHandle.Alloc(m_updateArray, GCHandleType.Pinned); | ||
151 | |||
152 | // if Debug, enable logging from the unmanaged code | ||
153 | if (m_log.IsDebugEnabled) | ||
154 | { | ||
155 | m_log.DebugFormat("{0}: Initialize: Setting debug callback for unmanaged code", LogHeader); | ||
156 | debugLogCallbackHandle = new BulletSimAPI.DebugLogCallback(BulletLogger); | ||
157 | BulletSimAPI.SetDebugLogCallback(debugLogCallbackHandle); | ||
158 | } | ||
159 | |||
160 | _taintedObjects = new List<TaintCallback>(); | ||
161 | |||
162 | mesher = meshmerizer; | ||
163 | // The bounding box for the simulated world | ||
164 | Vector3 worldExtent = new Vector3(Constants.RegionSize, Constants.RegionSize, 4096f); | ||
165 | |||
166 | // m_log.DebugFormat("{0}: Initialize: Calling BulletSimAPI.Initialize.", LogHeader); | ||
167 | m_worldID = BulletSimAPI.Initialize(worldExtent, m_paramsHandle.AddrOfPinnedObject(), | ||
168 | m_maxCollisionsPerFrame, m_collisionArrayPinnedHandle.AddrOfPinnedObject(), | ||
169 | m_maxUpdatesPerFrame, m_updateArrayPinnedHandle.AddrOfPinnedObject()); | ||
170 | |||
171 | m_initialized = true; | ||
172 | } | ||
173 | |||
174 | // All default parameter values are set here. There should be no values set in the | ||
175 | // variable definitions. | ||
176 | private void GetInitialParameterValues(IConfigSource config) | ||
177 | { | ||
178 | ConfigurationParameters parms = new ConfigurationParameters(); | ||
179 | |||
180 | _meshSculptedPrim = true; // mesh sculpted prims | ||
181 | _forceSimplePrimMeshing = false; // use complex meshing if called for | ||
182 | |||
183 | m_meshLOD = 32; | ||
184 | |||
185 | m_maxSubSteps = 10; | ||
186 | m_fixedTimeStep = 1f / 60f; | ||
187 | m_maxCollisionsPerFrame = 2048; | ||
188 | m_maxUpdatesPerFrame = 2048; | ||
189 | m_maximumObjectMass = 10000.01f; | ||
190 | |||
191 | parms.defaultFriction = 0.70f; | ||
192 | parms.defaultDensity = 10.000006836f; // Aluminum g/cm3 | ||
193 | parms.defaultRestitution = 0f; | ||
194 | parms.collisionMargin = 0.0f; | ||
195 | parms.gravity = -9.80665f; | ||
196 | |||
197 | parms.linearDamping = 0.0f; | ||
198 | parms.angularDamping = 0.0f; | ||
199 | parms.deactivationTime = 0.2f; | ||
200 | parms.linearSleepingThreshold = 0.8f; | ||
201 | parms.angularSleepingThreshold = 1.0f; | ||
202 | parms.ccdMotionThreshold = 0.5f; // set to zero to disable | ||
203 | parms.ccdSweptSphereRadius = 0.2f; | ||
204 | |||
205 | parms.terrainFriction = 0.85f; | ||
206 | parms.terrainHitFriction = 0.8f; | ||
207 | parms.terrainRestitution = 0.2f; | ||
208 | parms.avatarFriction = 0.85f; | ||
209 | parms.avatarDensity = 60f; | ||
210 | parms.avatarCapsuleRadius = 0.37f; | ||
211 | parms.avatarCapsuleHeight = 1.5f; // 2.140599f | ||
212 | |||
213 | if (config != null) | ||
214 | { | ||
215 | // If there are specifications in the ini file, use those values | ||
216 | // WHEN ADDING OR UPDATING THIS SECTION, BE SURE TO ALSO UPDATE OpenSimDefaults.ini | ||
217 | IConfig pConfig = config.Configs["BulletSim"]; | ||
218 | if (pConfig != null) | ||
219 | { | ||
220 | _meshSculptedPrim = pConfig.GetBoolean("MeshSculptedPrim", _meshSculptedPrim); | ||
221 | _forceSimplePrimMeshing = pConfig.GetBoolean("ForceSimplePrimMeshing", _forceSimplePrimMeshing); | ||
222 | |||
223 | m_meshLOD = pConfig.GetInt("MeshLevelOfDetail", m_meshLOD); | ||
224 | |||
225 | m_maxSubSteps = pConfig.GetInt("MaxSubSteps", m_maxSubSteps); | ||
226 | m_fixedTimeStep = pConfig.GetFloat("FixedTimeStep", m_fixedTimeStep); | ||
227 | m_maxCollisionsPerFrame = pConfig.GetInt("MaxCollisionsPerFrame", m_maxCollisionsPerFrame); | ||
228 | m_maxUpdatesPerFrame = pConfig.GetInt("MaxUpdatesPerFrame", m_maxUpdatesPerFrame); | ||
229 | m_maximumObjectMass = pConfig.GetFloat("MaxObjectMass", m_maximumObjectMass); | ||
230 | |||
231 | parms.defaultFriction = pConfig.GetFloat("DefaultFriction", parms.defaultFriction); | ||
232 | parms.defaultDensity = pConfig.GetFloat("DefaultDensity", parms.defaultDensity); | ||
233 | parms.defaultRestitution = pConfig.GetFloat("DefaultRestitution", parms.defaultRestitution); | ||
234 | parms.collisionMargin = pConfig.GetFloat("CollisionMargin", parms.collisionMargin); | ||
235 | parms.gravity = pConfig.GetFloat("Gravity", parms.gravity); | ||
236 | |||
237 | parms.linearDamping = pConfig.GetFloat("LinearDamping", parms.linearDamping); | ||
238 | parms.angularDamping = pConfig.GetFloat("AngularDamping", parms.angularDamping); | ||
239 | parms.deactivationTime = pConfig.GetFloat("DeactivationTime", parms.deactivationTime); | ||
240 | parms.linearSleepingThreshold = pConfig.GetFloat("LinearSleepingThreshold", parms.linearSleepingThreshold); | ||
241 | parms.angularSleepingThreshold = pConfig.GetFloat("AngularSleepingThreshold", parms.angularSleepingThreshold); | ||
242 | parms.ccdMotionThreshold = pConfig.GetFloat("CcdMotionThreshold", parms.ccdMotionThreshold); | ||
243 | parms.ccdSweptSphereRadius = pConfig.GetFloat("CcdSweptSphereRadius", parms.ccdSweptSphereRadius); | ||
244 | |||
245 | parms.terrainFriction = pConfig.GetFloat("TerrainFriction", parms.terrainFriction); | ||
246 | parms.terrainHitFriction = pConfig.GetFloat("TerrainHitFriction", parms.terrainHitFriction); | ||
247 | parms.terrainRestitution = pConfig.GetFloat("TerrainRestitution", parms.terrainRestitution); | ||
248 | parms.avatarFriction = pConfig.GetFloat("AvatarFriction", parms.avatarFriction); | ||
249 | parms.avatarDensity = pConfig.GetFloat("AvatarDensity", parms.avatarDensity); | ||
250 | parms.avatarCapsuleRadius = pConfig.GetFloat("AvatarCapsuleRadius", parms.avatarCapsuleRadius); | ||
251 | parms.avatarCapsuleHeight = pConfig.GetFloat("AvatarCapsuleHeight", parms.avatarCapsuleHeight); | ||
252 | } | ||
253 | } | ||
254 | m_params[0] = parms; | ||
255 | } | ||
256 | |||
257 | // Called directly from unmanaged code so don't do much | ||
258 | private void BulletLogger(string msg) | ||
259 | { | ||
260 | m_log.Debug("[BULLETS UNMANAGED]:" + msg); | ||
261 | } | ||
262 | |||
263 | public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying) | ||
264 | { | ||
265 | m_log.ErrorFormat("{0}: CALL TO AddAvatar in BSScene. NOT IMPLEMENTED", LogHeader); | ||
266 | return null; | ||
267 | } | ||
268 | |||
269 | public override PhysicsActor AddAvatar(uint localID, string avName, Vector3 position, Vector3 size, bool isFlying) | ||
270 | { | ||
271 | // m_log.DebugFormat("{0}: AddAvatar: {1}", LogHeader, avName); | ||
272 | BSCharacter actor = new BSCharacter(localID, avName, this, position, size, isFlying); | ||
273 | lock (m_avatars) m_avatars.Add(localID, actor); | ||
274 | return actor; | ||
275 | } | ||
276 | |||
277 | public override void RemoveAvatar(PhysicsActor actor) | ||
278 | { | ||
279 | // m_log.DebugFormat("{0}: RemoveAvatar", LogHeader); | ||
280 | if (actor is BSCharacter) | ||
281 | { | ||
282 | ((BSCharacter)actor).Destroy(); | ||
283 | } | ||
284 | try | ||
285 | { | ||
286 | lock (m_avatars) m_avatars.Remove(actor.LocalID); | ||
287 | } | ||
288 | catch (Exception e) | ||
289 | { | ||
290 | m_log.WarnFormat("{0}: Attempt to remove avatar that is not in physics scene: {1}", LogHeader, e); | ||
291 | } | ||
292 | } | ||
293 | |||
294 | public override void RemovePrim(PhysicsActor prim) | ||
295 | { | ||
296 | // m_log.DebugFormat("{0}: RemovePrim", LogHeader); | ||
297 | if (prim is BSPrim) | ||
298 | { | ||
299 | ((BSPrim)prim).Destroy(); | ||
300 | } | ||
301 | try | ||
302 | { | ||
303 | lock (m_prims) m_prims.Remove(prim.LocalID); | ||
304 | } | ||
305 | catch (Exception e) | ||
306 | { | ||
307 | m_log.WarnFormat("{0}: Attempt to remove prim that is not in physics scene: {1}", LogHeader, e); | ||
308 | } | ||
309 | } | ||
310 | |||
311 | public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, | ||
312 | Vector3 size, Quaternion rotation, bool isPhysical, uint localID) | ||
313 | { | ||
314 | // m_log.DebugFormat("{0}: AddPrimShape2: {1}", LogHeader, primName); | ||
315 | BSPrim prim = new BSPrim(localID, primName, this, position, size, rotation, pbs, isPhysical); | ||
316 | lock (m_prims) m_prims.Add(localID, prim); | ||
317 | return prim; | ||
318 | } | ||
319 | |||
320 | // This is a call from the simulator saying that some physical property has been updated. | ||
321 | // The BulletSim driver senses the changing of relevant properties so this taint | ||
322 | // information call is not needed. | ||
323 | public override void AddPhysicsActorTaint(PhysicsActor prim) { } | ||
324 | |||
325 | // Simulate one timestep | ||
326 | public override float Simulate(float timeStep) | ||
327 | { | ||
328 | int updatedEntityCount; | ||
329 | IntPtr updatedEntitiesPtr; | ||
330 | int collidersCount; | ||
331 | IntPtr collidersPtr; | ||
332 | |||
333 | // prevent simulation until we've been initialized | ||
334 | if (!m_initialized) return 10.0f; | ||
335 | |||
336 | // update the prim states while we know the physics engine is not busy | ||
337 | ProcessTaints(); | ||
338 | |||
339 | // Some of the prims operate with special vehicle properties | ||
340 | ProcessVehicles(timeStep); | ||
341 | ProcessTaints(); // the vehicles might have added taints | ||
342 | |||
343 | // step the physical world one interval | ||
344 | m_simulationStep++; | ||
345 | int numSubSteps = BulletSimAPI.PhysicsStep(m_worldID, timeStep, m_maxSubSteps, m_fixedTimeStep, | ||
346 | out updatedEntityCount, out updatedEntitiesPtr, out collidersCount, out collidersPtr); | ||
347 | |||
348 | // Don't have to use the pointers passed back since we know it is the same pinned memory we passed in | ||
349 | |||
350 | // Get a value for 'now' so all the collision and update routines don't have to get their own | ||
351 | m_simulationNowTime = Util.EnvironmentTickCount(); | ||
352 | |||
353 | // If there were collisions, process them by sending the event to the prim. | ||
354 | // Collisions must be processed before updates. | ||
355 | if (collidersCount > 0) | ||
356 | { | ||
357 | for (int ii = 0; ii < collidersCount; ii++) | ||
358 | { | ||
359 | uint cA = m_collisionArray[ii].aID; | ||
360 | uint cB = m_collisionArray[ii].bID; | ||
361 | Vector3 point = m_collisionArray[ii].point; | ||
362 | Vector3 normal = m_collisionArray[ii].normal; | ||
363 | SendCollision(cA, cB, point, normal, 0.01f); | ||
364 | SendCollision(cB, cA, point, -normal, 0.01f); | ||
365 | } | ||
366 | } | ||
367 | |||
368 | // If any of the objects had updated properties, tell the object it has been changed by the physics engine | ||
369 | if (updatedEntityCount > 0) | ||
370 | { | ||
371 | for (int ii = 0; ii < updatedEntityCount; ii++) | ||
372 | { | ||
373 | EntityProperties entprop = m_updateArray[ii]; | ||
374 | // m_log.DebugFormat("{0}: entprop[{1}]: id={2}, pos={3}", LogHeader, ii, entprop.ID, entprop.Position); | ||
375 | BSCharacter actor; | ||
376 | if (m_avatars.TryGetValue(entprop.ID, out actor)) | ||
377 | { | ||
378 | actor.UpdateProperties(entprop); | ||
379 | continue; | ||
380 | } | ||
381 | BSPrim prim; | ||
382 | if (m_prims.TryGetValue(entprop.ID, out prim)) | ||
383 | { | ||
384 | prim.UpdateProperties(entprop); | ||
385 | } | ||
386 | } | ||
387 | } | ||
388 | |||
389 | // FIX THIS: fps calculation wrong. This calculation always returns about 1 in normal operation. | ||
390 | return timeStep / (numSubSteps * m_fixedTimeStep) * 1000f; | ||
391 | } | ||
392 | |||
393 | // Something has collided | ||
394 | private void SendCollision(uint localID, uint collidingWith, Vector3 collidePoint, Vector3 collideNormal, float penitration) | ||
395 | { | ||
396 | if (localID == TERRAIN_ID || localID == GROUNDPLANE_ID) | ||
397 | { | ||
398 | return; // don't send collisions to the terrain | ||
399 | } | ||
400 | |||
401 | ActorTypes type = ActorTypes.Prim; | ||
402 | if (collidingWith == TERRAIN_ID || collidingWith == GROUNDPLANE_ID) | ||
403 | type = ActorTypes.Ground; | ||
404 | else if (m_avatars.ContainsKey(collidingWith)) | ||
405 | type = ActorTypes.Agent; | ||
406 | |||
407 | BSPrim prim; | ||
408 | if (m_prims.TryGetValue(localID, out prim)) { | ||
409 | prim.Collide(collidingWith, type, collidePoint, collideNormal, penitration); | ||
410 | return; | ||
411 | } | ||
412 | BSCharacter actor; | ||
413 | if (m_avatars.TryGetValue(localID, out actor)) { | ||
414 | actor.Collide(collidingWith, type, collidePoint, collideNormal, penitration); | ||
415 | return; | ||
416 | } | ||
417 | return; | ||
418 | } | ||
419 | |||
420 | public override void GetResults() { } | ||
421 | |||
422 | public override void SetTerrain(float[] heightMap) { | ||
423 | m_heightMap = heightMap; | ||
424 | this.TaintedObject(delegate() | ||
425 | { | ||
426 | BulletSimAPI.SetHeightmap(m_worldID, m_heightMap); | ||
427 | }); | ||
428 | } | ||
429 | |||
430 | public float GetTerrainHeightAtXY(float tX, float tY) | ||
431 | { | ||
432 | return m_heightMap[((int)tX) * Constants.RegionSize + ((int)tY)]; | ||
433 | } | ||
434 | |||
435 | public override void SetWaterLevel(float baseheight) | ||
436 | { | ||
437 | m_waterLevel = baseheight; | ||
438 | } | ||
439 | public float GetWaterLevel() | ||
440 | { | ||
441 | return m_waterLevel; | ||
442 | } | ||
443 | |||
444 | public override void DeleteTerrain() | ||
445 | { | ||
446 | m_log.DebugFormat("{0}: DeleteTerrain()", LogHeader); | ||
447 | } | ||
448 | |||
449 | public override void Dispose() | ||
450 | { | ||
451 | m_log.DebugFormat("{0}: Dispose()", LogHeader); | ||
452 | } | ||
453 | |||
454 | public override Dictionary<uint, float> GetTopColliders() | ||
455 | { | ||
456 | return new Dictionary<uint, float>(); | ||
457 | } | ||
458 | |||
459 | public override bool IsThreaded { get { return false; } } | ||
460 | |||
461 | /// <summary> | ||
462 | /// Routine to figure out if we need to mesh this prim with our mesher | ||
463 | /// </summary> | ||
464 | /// <param name="pbs"></param> | ||
465 | /// <returns>true if the prim needs meshing</returns> | ||
466 | public bool NeedsMeshing(PrimitiveBaseShape pbs) | ||
467 | { | ||
468 | // most of this is redundant now as the mesher will return null if it cant mesh a prim | ||
469 | // but we still need to check for sculptie meshing being enabled so this is the most | ||
470 | // convenient place to do it for now... | ||
471 | |||
472 | // int iPropertiesNotSupportedDefault = 0; | ||
473 | |||
474 | if (pbs.SculptEntry && !_meshSculptedPrim) | ||
475 | { | ||
476 | // Render sculpties as boxes | ||
477 | return false; | ||
478 | } | ||
479 | |||
480 | // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since Bullet | ||
481 | // can use an internal representation for the prim | ||
482 | if (!_forceSimplePrimMeshing) | ||
483 | { | ||
484 | // m_log.DebugFormat("{0}: NeedsMeshing: simple mesh: profshape={1}, curve={2}", LogHeader, pbs.ProfileShape, pbs.PathCurve); | ||
485 | if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight) | ||
486 | || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 | ||
487 | && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z)) | ||
488 | { | ||
489 | |||
490 | if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0 | ||
491 | && pbs.ProfileHollow == 0 | ||
492 | && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0 | ||
493 | && pbs.PathBegin == 0 && pbs.PathEnd == 0 | ||
494 | && pbs.PathTaperX == 0 && pbs.PathTaperY == 0 | ||
495 | && pbs.PathScaleX == 100 && pbs.PathScaleY == 100 | ||
496 | && pbs.PathShearX == 0 && pbs.PathShearY == 0) | ||
497 | { | ||
498 | return false; | ||
499 | } | ||
500 | } | ||
501 | } | ||
502 | |||
503 | /* TODO: verify that the mesher will now do all these shapes | ||
504 | if (pbs.ProfileHollow != 0) | ||
505 | iPropertiesNotSupportedDefault++; | ||
506 | |||
507 | if ((pbs.PathBegin != 0) || pbs.PathEnd != 0) | ||
508 | iPropertiesNotSupportedDefault++; | ||
509 | |||
510 | if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0)) | ||
511 | iPropertiesNotSupportedDefault++; | ||
512 | |||
513 | if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0) | ||
514 | iPropertiesNotSupportedDefault++; | ||
515 | |||
516 | if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100)) | ||
517 | iPropertiesNotSupportedDefault++; | ||
518 | |||
519 | if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0)) | ||
520 | iPropertiesNotSupportedDefault++; | ||
521 | |||
522 | if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight) | ||
523 | iPropertiesNotSupportedDefault++; | ||
524 | |||
525 | if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 && (pbs.Scale.X != pbs.Scale.Y || pbs.Scale.Y != pbs.Scale.Z || pbs.Scale.Z != pbs.Scale.X)) | ||
526 | iPropertiesNotSupportedDefault++; | ||
527 | |||
528 | if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1) | ||
529 | iPropertiesNotSupportedDefault++; | ||
530 | |||
531 | // test for torus | ||
532 | if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square) | ||
533 | { | ||
534 | if (pbs.PathCurve == (byte)Extrusion.Curve1) | ||
535 | { | ||
536 | iPropertiesNotSupportedDefault++; | ||
537 | } | ||
538 | } | ||
539 | else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle) | ||
540 | { | ||
541 | if (pbs.PathCurve == (byte)Extrusion.Straight) | ||
542 | { | ||
543 | iPropertiesNotSupportedDefault++; | ||
544 | } | ||
545 | // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits | ||
546 | else if (pbs.PathCurve == (byte)Extrusion.Curve1) | ||
547 | { | ||
548 | iPropertiesNotSupportedDefault++; | ||
549 | } | ||
550 | } | ||
551 | else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle) | ||
552 | { | ||
553 | if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2) | ||
554 | { | ||
555 | iPropertiesNotSupportedDefault++; | ||
556 | } | ||
557 | } | ||
558 | else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle) | ||
559 | { | ||
560 | if (pbs.PathCurve == (byte)Extrusion.Straight) | ||
561 | { | ||
562 | iPropertiesNotSupportedDefault++; | ||
563 | } | ||
564 | else if (pbs.PathCurve == (byte)Extrusion.Curve1) | ||
565 | { | ||
566 | iPropertiesNotSupportedDefault++; | ||
567 | } | ||
568 | } | ||
569 | if (iPropertiesNotSupportedDefault == 0) | ||
570 | { | ||
571 | return false; | ||
572 | } | ||
573 | */ | ||
574 | return true; | ||
575 | } | ||
576 | |||
577 | // The calls to the PhysicsActors can't directly call into the physics engine | ||
578 | // because it might be busy. We we delay changes to a known time. | ||
579 | // We rely on C#'s closure to save and restore the context for the delegate. | ||
580 | public void TaintedObject(TaintCallback callback) | ||
581 | { | ||
582 | lock (_taintLock) | ||
583 | _taintedObjects.Add(callback); | ||
584 | return; | ||
585 | } | ||
586 | |||
587 | // When someone tries to change a property on a BSPrim or BSCharacter, the object queues | ||
588 | // a callback into itself to do the actual property change. That callback is called | ||
589 | // here just before the physics engine is called to step the simulation. | ||
590 | public void ProcessTaints() | ||
591 | { | ||
592 | if (_taintedObjects.Count > 0) // save allocating new list if there is nothing to process | ||
593 | { | ||
594 | // swizzle a new list into the list location so we can process what's there | ||
595 | List<TaintCallback> oldList; | ||
596 | lock (_taintLock) | ||
597 | { | ||
598 | oldList = _taintedObjects; | ||
599 | _taintedObjects = new List<TaintCallback>(); | ||
600 | } | ||
601 | |||
602 | foreach (TaintCallback callback in oldList) | ||
603 | { | ||
604 | try | ||
605 | { | ||
606 | callback(); | ||
607 | } | ||
608 | catch (Exception e) | ||
609 | { | ||
610 | m_log.ErrorFormat("{0}: ProcessTaints: Exception: {1}", LogHeader, e); | ||
611 | } | ||
612 | } | ||
613 | oldList.Clear(); | ||
614 | } | ||
615 | } | ||
616 | |||
617 | #region Vehicles | ||
618 | // Make so the scene will call this prim for vehicle actions each tick. | ||
619 | // Safe to call if prim is already in the vehicle list. | ||
620 | public void AddVehiclePrim(BSPrim vehicle) | ||
621 | { | ||
622 | lock (m_vehicles) | ||
623 | { | ||
624 | if (!m_vehicles.Contains(vehicle)) | ||
625 | { | ||
626 | m_vehicles.Add(vehicle); | ||
627 | } | ||
628 | } | ||
629 | } | ||
630 | |||
631 | // Remove a prim from our list of vehicles. | ||
632 | // Safe to call if the prim is not in the vehicle list. | ||
633 | public void RemoveVehiclePrim(BSPrim vehicle) | ||
634 | { | ||
635 | lock (m_vehicles) | ||
636 | { | ||
637 | if (m_vehicles.Contains(vehicle)) | ||
638 | { | ||
639 | m_vehicles.Remove(vehicle); | ||
640 | } | ||
641 | } | ||
642 | } | ||
643 | |||
644 | // Some prims have extra vehicle actions | ||
645 | // no locking because only called when physics engine is not busy | ||
646 | private void ProcessVehicles(float timeStep) | ||
647 | { | ||
648 | foreach (BSPrim prim in m_vehicles) | ||
649 | { | ||
650 | prim.StepVehicle(timeStep); | ||
651 | } | ||
652 | } | ||
653 | #endregion Vehicles | ||
654 | } | ||
655 | } | ||
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs new file mode 100644 index 0000000..819fce1 --- /dev/null +++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs | |||
@@ -0,0 +1,234 @@ | |||
1 | /* | ||
2 | * Copyright (c) Contributors, http://opensimulator.org/ | ||
3 | * See CONTRIBUTORS.TXT for a full list of copyright holders. | ||
4 | * | ||
5 | * Redistribution and use in source and binary forms, with or without | ||
6 | * modification, are permitted provided that the following conditions are met: | ||
7 | * * Redistributions of source code must retain the above copyright | ||
8 | * notice, this list of conditions and the following disclaimer. | ||
9 | * * Redistributions in binary form must reproduce the above copyrightD | ||
10 | * notice, this list of conditions and the following disclaimer in the | ||
11 | * documentation and/or other materials provided with the distribution. | ||
12 | * * Neither the name of the OpenSimulator Project nor the | ||
13 | * names of its contributors may be used to endorse or promote products | ||
14 | * derived from this software without specific prior written permission. | ||
15 | * | ||
16 | * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY | ||
17 | * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED | ||
18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE | ||
19 | * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY | ||
20 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES | ||
21 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
22 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND | ||
23 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
24 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS | ||
25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. | ||
26 | */ | ||
27 | using System; | ||
28 | using System.Runtime.InteropServices; | ||
29 | using System.Security; | ||
30 | using System.Text; | ||
31 | using OpenMetaverse; | ||
32 | |||
33 | namespace OpenSim.Region.Physics.BulletSPlugin { | ||
34 | |||
35 | [StructLayout(LayoutKind.Sequential)] | ||
36 | public struct ConvexHull | ||
37 | { | ||
38 | Vector3 Offset; | ||
39 | int VertexCount; | ||
40 | Vector3[] Vertices; | ||
41 | } | ||
42 | [StructLayout(LayoutKind.Sequential)] | ||
43 | public struct ShapeData | ||
44 | { | ||
45 | public enum PhysicsShapeType | ||
46 | { | ||
47 | SHAPE_AVATAR = 0, | ||
48 | SHAPE_BOX = 1, | ||
49 | SHAPE_CONE = 2, | ||
50 | SHAPE_CYLINDER = 3, | ||
51 | SHAPE_SPHERE = 4, | ||
52 | SHAPE_HULL = 5 | ||
53 | }; | ||
54 | // note that bools are passed as ints since bool size changes by language | ||
55 | public const int numericTrue = 1; | ||
56 | public const int numericFalse = 0; | ||
57 | public uint ID; | ||
58 | public PhysicsShapeType Type; | ||
59 | public Vector3 Position; | ||
60 | public Quaternion Rotation; | ||
61 | public Vector3 Velocity; | ||
62 | public Vector3 Scale; | ||
63 | public float Mass; | ||
64 | public float Buoyancy; | ||
65 | public System.UInt64 MeshKey; | ||
66 | public float Friction; | ||
67 | public float Restitution; | ||
68 | public int Collidable; | ||
69 | public int Static; // true if a static object. Otherwise gravity, etc. | ||
70 | } | ||
71 | [StructLayout(LayoutKind.Sequential)] | ||
72 | public struct SweepHit | ||
73 | { | ||
74 | public uint ID; | ||
75 | public float Fraction; | ||
76 | public Vector3 Normal; | ||
77 | public Vector3 Point; | ||
78 | } | ||
79 | [StructLayout(LayoutKind.Sequential)] | ||
80 | public struct RaycastHit | ||
81 | { | ||
82 | public uint ID; | ||
83 | public float Fraction; | ||
84 | public Vector3 Normal; | ||
85 | } | ||
86 | [StructLayout(LayoutKind.Sequential)] | ||
87 | public struct CollisionDesc | ||
88 | { | ||
89 | public uint aID; | ||
90 | public uint bID; | ||
91 | public Vector3 point; | ||
92 | public Vector3 normal; | ||
93 | } | ||
94 | [StructLayout(LayoutKind.Sequential)] | ||
95 | public struct EntityProperties | ||
96 | { | ||
97 | public uint ID; | ||
98 | public Vector3 Position; | ||
99 | public Quaternion Rotation; | ||
100 | public Vector3 Velocity; | ||
101 | public Vector3 Acceleration; | ||
102 | public Vector3 RotationalVelocity; | ||
103 | } | ||
104 | |||
105 | // Format of this structure must match the definition in the C++ code | ||
106 | [StructLayout(LayoutKind.Sequential)] | ||
107 | public struct ConfigurationParameters | ||
108 | { | ||
109 | public float defaultFriction; | ||
110 | public float defaultDensity; | ||
111 | public float defaultRestitution; | ||
112 | public float collisionMargin; | ||
113 | public float gravity; | ||
114 | |||
115 | public float linearDamping; | ||
116 | public float angularDamping; | ||
117 | public float deactivationTime; | ||
118 | public float linearSleepingThreshold; | ||
119 | public float angularSleepingThreshold; | ||
120 | public float ccdMotionThreshold; | ||
121 | public float ccdSweptSphereRadius; | ||
122 | |||
123 | public float terrainFriction; | ||
124 | public float terrainHitFriction; | ||
125 | public float terrainRestitution; | ||
126 | public float avatarFriction; | ||
127 | public float avatarDensity; | ||
128 | public float avatarRestitution; | ||
129 | public float avatarCapsuleRadius; | ||
130 | public float avatarCapsuleHeight; | ||
131 | } | ||
132 | |||
133 | static class BulletSimAPI { | ||
134 | |||
135 | [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] | ||
136 | public static extern uint Initialize(Vector3 maxPosition, IntPtr parms, | ||
137 | int maxCollisions, IntPtr collisionArray, | ||
138 | int maxUpdates, IntPtr updateArray); | ||
139 | |||
140 | [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] | ||
141 | public static extern void SetHeightmap(uint worldID, [MarshalAs(UnmanagedType.LPArray)] float[] heightMap); | ||
142 | |||
143 | [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] | ||
144 | public static extern void Shutdown(uint worldID); | ||
145 | |||
146 | |||
147 | [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] | ||
148 | public static extern int PhysicsStep(uint worldID, float timeStep, int maxSubSteps, float fixedTimeStep, | ||
149 | out int updatedEntityCount, | ||
150 | out IntPtr updatedEntitiesPtr, | ||
151 | out int collidersCount, | ||
152 | out IntPtr collidersPtr); | ||
153 | |||
154 | [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] | ||
155 | public static extern bool CreateHull(uint worldID, System.UInt64 meshKey, int hullCount, | ||
156 | [MarshalAs(UnmanagedType.LPArray)] float[] hulls | ||
157 | ); | ||
158 | |||
159 | [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] | ||
160 | public static extern bool DestroyHull(uint worldID, System.UInt64 meshKey); | ||
161 | |||
162 | [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] | ||
163 | public static extern bool CreateObject(uint worldID, ShapeData shapeData); | ||
164 | |||
165 | [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] | ||
166 | public static extern void CreateLinkset(uint worldID, int objectCount, ShapeData[] shapeDatas); | ||
167 | |||
168 | [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] | ||
169 | public static extern void AddConstraint(uint worldID, uint id1, uint id2, | ||
170 | Vector3 frame1, Quaternion frame1rot, | ||
171 | Vector3 frame2, Quaternion frame2rot, | ||
172 | Vector3 lowLinear, Vector3 hiLinear, Vector3 lowAngular, Vector3 hiAngular); | ||
173 | |||
174 | [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] | ||
175 | public static extern bool RemoveConstraintByID(uint worldID, uint id1); | ||
176 | |||
177 | [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] | ||
178 | public static extern bool RemoveConstraint(uint worldID, uint id1, uint id2); | ||
179 | |||
180 | [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] | ||
181 | public static extern Vector3 GetObjectPosition(uint WorldID, uint id); | ||
182 | |||
183 | [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] | ||
184 | public static extern bool SetObjectTranslation(uint worldID, uint id, Vector3 position, Quaternion rotation); | ||
185 | |||
186 | [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] | ||
187 | public static extern bool SetObjectVelocity(uint worldID, uint id, Vector3 velocity); | ||
188 | |||
189 | [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] | ||
190 | public static extern bool SetObjectAngularVelocity(uint worldID, uint id, Vector3 angularVelocity); | ||
191 | |||
192 | [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] | ||
193 | public static extern bool SetObjectForce(uint worldID, uint id, Vector3 force); | ||
194 | |||
195 | [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] | ||
196 | public static extern bool SetObjectScaleMass(uint worldID, uint id, Vector3 scale, float mass, bool isDynamic); | ||
197 | |||
198 | [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] | ||
199 | public static extern bool SetObjectCollidable(uint worldID, uint id, bool phantom); | ||
200 | |||
201 | [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] | ||
202 | public static extern bool SetObjectDynamic(uint worldID, uint id, bool isDynamic, float mass); | ||
203 | |||
204 | [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] | ||
205 | public static extern bool SetObjectGhost(uint worldID, uint id, bool ghostly); | ||
206 | |||
207 | [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] | ||
208 | public static extern bool SetObjectProperties(uint worldID, uint id, bool isStatic, bool isSolid, bool genCollisions, float mass); | ||
209 | |||
210 | [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] | ||
211 | public static extern bool SetObjectBuoyancy(uint worldID, uint id, float buoyancy); | ||
212 | |||
213 | [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] | ||
214 | public static extern bool HasObject(uint worldID, uint id); | ||
215 | |||
216 | [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] | ||
217 | public static extern bool DestroyObject(uint worldID, uint id); | ||
218 | |||
219 | [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] | ||
220 | public static extern SweepHit ConvexSweepTest(uint worldID, uint id, Vector3 to, float extraMargin); | ||
221 | |||
222 | [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] | ||
223 | public static extern RaycastHit RayTest(uint worldID, uint id, Vector3 from, Vector3 to); | ||
224 | |||
225 | [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] | ||
226 | public static extern Vector3 RecoverFromPenetration(uint worldID, uint id); | ||
227 | |||
228 | // Log a debug message | ||
229 | [UnmanagedFunctionPointer(CallingConvention.Cdecl)] | ||
230 | public delegate void DebugLogCallback([MarshalAs(UnmanagedType.LPStr)]string msg); | ||
231 | [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity] | ||
232 | public static extern void SetDebugLogCallback(DebugLogCallback callback); | ||
233 | } | ||
234 | } | ||
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/CTri.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/CTri.cs new file mode 100644 index 0000000..4d84c44 --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/CTri.cs | |||
@@ -0,0 +1,341 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | using System.Collections.Generic; | ||
30 | |||
31 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
32 | { | ||
33 | public class Wpoint | ||
34 | { | ||
35 | public float3 mPoint; | ||
36 | public float mWeight; | ||
37 | |||
38 | public Wpoint(float3 p, float w) | ||
39 | { | ||
40 | mPoint = p; | ||
41 | mWeight = w; | ||
42 | } | ||
43 | } | ||
44 | |||
45 | public class CTri | ||
46 | { | ||
47 | private const int WSCALE = 4; | ||
48 | |||
49 | public float3 mP1; | ||
50 | public float3 mP2; | ||
51 | public float3 mP3; | ||
52 | public float3 mNear1; | ||
53 | public float3 mNear2; | ||
54 | public float3 mNear3; | ||
55 | public float3 mNormal; | ||
56 | public float mPlaneD; | ||
57 | public float mConcavity; | ||
58 | public float mC1; | ||
59 | public float mC2; | ||
60 | public float mC3; | ||
61 | public int mI1; | ||
62 | public int mI2; | ||
63 | public int mI3; | ||
64 | public int mProcessed; // already been added... | ||
65 | |||
66 | public CTri(float3 p1, float3 p2, float3 p3, int i1, int i2, int i3) | ||
67 | { | ||
68 | mProcessed = 0; | ||
69 | mI1 = i1; | ||
70 | mI2 = i2; | ||
71 | mI3 = i3; | ||
72 | |||
73 | mP1 = new float3(p1); | ||
74 | mP2 = new float3(p2); | ||
75 | mP3 = new float3(p3); | ||
76 | |||
77 | mNear1 = new float3(); | ||
78 | mNear2 = new float3(); | ||
79 | mNear3 = new float3(); | ||
80 | |||
81 | mNormal = new float3(); | ||
82 | mPlaneD = mNormal.ComputePlane(mP1, mP2, mP3); | ||
83 | } | ||
84 | |||
85 | public float Facing(CTri t) | ||
86 | { | ||
87 | return float3.dot(mNormal, t.mNormal); | ||
88 | } | ||
89 | |||
90 | public bool clip(float3 start, ref float3 end) | ||
91 | { | ||
92 | float3 sect = new float3(); | ||
93 | bool hit = lineIntersectsTriangle(start, end, mP1, mP2, mP3, ref sect); | ||
94 | |||
95 | if (hit) | ||
96 | end = sect; | ||
97 | return hit; | ||
98 | } | ||
99 | |||
100 | public bool Concave(float3 p, ref float distance, ref float3 n) | ||
101 | { | ||
102 | n.NearestPointInTriangle(p, mP1, mP2, mP3); | ||
103 | distance = p.Distance(n); | ||
104 | return true; | ||
105 | } | ||
106 | |||
107 | public void addTri(int[] indices, int i1, int i2, int i3, ref int tcount) | ||
108 | { | ||
109 | indices[tcount * 3 + 0] = i1; | ||
110 | indices[tcount * 3 + 1] = i2; | ||
111 | indices[tcount * 3 + 2] = i3; | ||
112 | tcount++; | ||
113 | } | ||
114 | |||
115 | public float getVolume() | ||
116 | { | ||
117 | int[] indices = new int[8 * 3]; | ||
118 | |||
119 | int tcount = 0; | ||
120 | |||
121 | addTri(indices, 0, 1, 2, ref tcount); | ||
122 | addTri(indices, 3, 4, 5, ref tcount); | ||
123 | |||
124 | addTri(indices, 0, 3, 4, ref tcount); | ||
125 | addTri(indices, 0, 4, 1, ref tcount); | ||
126 | |||
127 | addTri(indices, 1, 4, 5, ref tcount); | ||
128 | addTri(indices, 1, 5, 2, ref tcount); | ||
129 | |||
130 | addTri(indices, 0, 3, 5, ref tcount); | ||
131 | addTri(indices, 0, 5, 2, ref tcount); | ||
132 | |||
133 | List<float3> vertices = new List<float3> { mP1, mP2, mP3, mNear1, mNear2, mNear3 }; | ||
134 | List<int> indexList = new List<int>(indices); | ||
135 | |||
136 | float v = Concavity.computeMeshVolume(vertices, indexList); | ||
137 | return v; | ||
138 | } | ||
139 | |||
140 | public float raySect(float3 p, float3 dir, ref float3 sect) | ||
141 | { | ||
142 | float4 plane = new float4(); | ||
143 | |||
144 | plane.x = mNormal.x; | ||
145 | plane.y = mNormal.y; | ||
146 | plane.z = mNormal.z; | ||
147 | plane.w = mPlaneD; | ||
148 | |||
149 | float3 dest = p + dir * 100000f; | ||
150 | |||
151 | intersect(p, dest, ref sect, plane); | ||
152 | |||
153 | return sect.Distance(p); // return the intersection distance | ||
154 | } | ||
155 | |||
156 | public float planeDistance(float3 p) | ||
157 | { | ||
158 | float4 plane = new float4(); | ||
159 | |||
160 | plane.x = mNormal.x; | ||
161 | plane.y = mNormal.y; | ||
162 | plane.z = mNormal.z; | ||
163 | plane.w = mPlaneD; | ||
164 | |||
165 | return DistToPt(p, plane); | ||
166 | } | ||
167 | |||
168 | public bool samePlane(CTri t) | ||
169 | { | ||
170 | const float THRESH = 0.001f; | ||
171 | float dd = Math.Abs(t.mPlaneD - mPlaneD); | ||
172 | if (dd > THRESH) | ||
173 | return false; | ||
174 | dd = Math.Abs(t.mNormal.x - mNormal.x); | ||
175 | if (dd > THRESH) | ||
176 | return false; | ||
177 | dd = Math.Abs(t.mNormal.y - mNormal.y); | ||
178 | if (dd > THRESH) | ||
179 | return false; | ||
180 | dd = Math.Abs(t.mNormal.z - mNormal.z); | ||
181 | if (dd > THRESH) | ||
182 | return false; | ||
183 | return true; | ||
184 | } | ||
185 | |||
186 | public bool hasIndex(int i) | ||
187 | { | ||
188 | if (i == mI1 || i == mI2 || i == mI3) | ||
189 | return true; | ||
190 | return false; | ||
191 | } | ||
192 | |||
193 | public bool sharesEdge(CTri t) | ||
194 | { | ||
195 | bool ret = false; | ||
196 | uint count = 0; | ||
197 | |||
198 | if (t.hasIndex(mI1)) | ||
199 | count++; | ||
200 | if (t.hasIndex(mI2)) | ||
201 | count++; | ||
202 | if (t.hasIndex(mI3)) | ||
203 | count++; | ||
204 | |||
205 | if (count >= 2) | ||
206 | ret = true; | ||
207 | |||
208 | return ret; | ||
209 | } | ||
210 | |||
211 | public float area() | ||
212 | { | ||
213 | float a = mConcavity * mP1.Area(mP2, mP3); | ||
214 | return a; | ||
215 | } | ||
216 | |||
217 | public void addWeighted(List<Wpoint> list) | ||
218 | { | ||
219 | Wpoint p1 = new Wpoint(mP1, mC1); | ||
220 | Wpoint p2 = new Wpoint(mP2, mC2); | ||
221 | Wpoint p3 = new Wpoint(mP3, mC3); | ||
222 | |||
223 | float3 d1 = mNear1 - mP1; | ||
224 | float3 d2 = mNear2 - mP2; | ||
225 | float3 d3 = mNear3 - mP3; | ||
226 | |||
227 | d1 *= WSCALE; | ||
228 | d2 *= WSCALE; | ||
229 | d3 *= WSCALE; | ||
230 | |||
231 | d1 = d1 + mP1; | ||
232 | d2 = d2 + mP2; | ||
233 | d3 = d3 + mP3; | ||
234 | |||
235 | Wpoint p4 = new Wpoint(d1, mC1); | ||
236 | Wpoint p5 = new Wpoint(d2, mC2); | ||
237 | Wpoint p6 = new Wpoint(d3, mC3); | ||
238 | |||
239 | list.Add(p1); | ||
240 | list.Add(p2); | ||
241 | list.Add(p3); | ||
242 | |||
243 | list.Add(p4); | ||
244 | list.Add(p5); | ||
245 | list.Add(p6); | ||
246 | } | ||
247 | |||
248 | private static float DistToPt(float3 p, float4 plane) | ||
249 | { | ||
250 | float x = p.x; | ||
251 | float y = p.y; | ||
252 | float z = p.z; | ||
253 | float d = x*plane.x + y*plane.y + z*plane.z + plane.w; | ||
254 | return d; | ||
255 | } | ||
256 | |||
257 | private static void intersect(float3 p1, float3 p2, ref float3 split, float4 plane) | ||
258 | { | ||
259 | float dp1 = DistToPt(p1, plane); | ||
260 | |||
261 | float3 dir = new float3(); | ||
262 | dir.x = p2[0] - p1[0]; | ||
263 | dir.y = p2[1] - p1[1]; | ||
264 | dir.z = p2[2] - p1[2]; | ||
265 | |||
266 | float dot1 = dir[0] * plane[0] + dir[1] * plane[1] + dir[2] * plane[2]; | ||
267 | float dot2 = dp1 - plane[3]; | ||
268 | |||
269 | float t = -(plane[3] + dot2) / dot1; | ||
270 | |||
271 | split.x = (dir[0] * t) + p1[0]; | ||
272 | split.y = (dir[1] * t) + p1[1]; | ||
273 | split.z = (dir[2] * t) + p1[2]; | ||
274 | } | ||
275 | |||
276 | private static bool rayIntersectsTriangle(float3 p, float3 d, float3 v0, float3 v1, float3 v2, out float t) | ||
277 | { | ||
278 | t = 0f; | ||
279 | |||
280 | float3 e1, e2, h, s, q; | ||
281 | float a, f, u, v; | ||
282 | |||
283 | e1 = v1 - v0; | ||
284 | e2 = v2 - v0; | ||
285 | h = float3.cross(d, e2); | ||
286 | a = float3.dot(e1, h); | ||
287 | |||
288 | if (a > -0.00001f && a < 0.00001f) | ||
289 | return false; | ||
290 | |||
291 | f = 1f / a; | ||
292 | s = p - v0; | ||
293 | u = f * float3.dot(s, h); | ||
294 | |||
295 | if (u < 0.0f || u > 1.0f) | ||
296 | return false; | ||
297 | |||
298 | q = float3.cross(s, e1); | ||
299 | v = f * float3.dot(d, q); | ||
300 | if (v < 0.0f || u + v > 1.0f) | ||
301 | return false; | ||
302 | |||
303 | // at this stage we can compute t to find out where | ||
304 | // the intersection point is on the line | ||
305 | t = f * float3.dot(e2, q); | ||
306 | if (t > 0f) // ray intersection | ||
307 | return true; | ||
308 | else // this means that there is a line intersection but not a ray intersection | ||
309 | return false; | ||
310 | } | ||
311 | |||
312 | private static bool lineIntersectsTriangle(float3 rayStart, float3 rayEnd, float3 p1, float3 p2, float3 p3, ref float3 sect) | ||
313 | { | ||
314 | float3 dir = rayEnd - rayStart; | ||
315 | |||
316 | float d = (float)Math.Sqrt(dir[0] * dir[0] + dir[1] * dir[1] + dir[2] * dir[2]); | ||
317 | float r = 1.0f / d; | ||
318 | |||
319 | dir *= r; | ||
320 | |||
321 | float t; | ||
322 | bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, out t); | ||
323 | |||
324 | if (ret) | ||
325 | { | ||
326 | if (t > d) | ||
327 | { | ||
328 | sect.x = rayStart.x + dir.x * t; | ||
329 | sect.y = rayStart.y + dir.y * t; | ||
330 | sect.z = rayStart.z + dir.z * t; | ||
331 | } | ||
332 | else | ||
333 | { | ||
334 | ret = false; | ||
335 | } | ||
336 | } | ||
337 | |||
338 | return ret; | ||
339 | } | ||
340 | } | ||
341 | } | ||
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/Concavity.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/Concavity.cs new file mode 100644 index 0000000..cc6383a --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/Concavity.cs | |||
@@ -0,0 +1,233 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | using System.Collections.Generic; | ||
30 | using System.Text; | ||
31 | |||
32 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
33 | { | ||
34 | public static class Concavity | ||
35 | { | ||
36 | // compute's how 'concave' this object is and returns the total volume of the | ||
37 | // convex hull as well as the volume of the 'concavity' which was found. | ||
38 | public static float computeConcavity(List<float3> vertices, List<int> indices, ref float4 plane, ref float volume) | ||
39 | { | ||
40 | float cret = 0f; | ||
41 | volume = 1f; | ||
42 | |||
43 | HullResult result = new HullResult(); | ||
44 | HullDesc desc = new HullDesc(); | ||
45 | |||
46 | desc.MaxFaces = 256; | ||
47 | desc.MaxVertices = 256; | ||
48 | desc.SetHullFlag(HullFlag.QF_TRIANGLES); | ||
49 | desc.Vertices = vertices; | ||
50 | |||
51 | HullError ret = HullUtils.CreateConvexHull(desc, ref result); | ||
52 | |||
53 | if (ret == HullError.QE_OK) | ||
54 | { | ||
55 | volume = computeMeshVolume2(result.OutputVertices, result.Indices); | ||
56 | |||
57 | // ok..now..for each triangle on the original mesh.. | ||
58 | // we extrude the points to the nearest point on the hull. | ||
59 | List<CTri> tris = new List<CTri>(); | ||
60 | |||
61 | for (int i = 0; i < result.Indices.Count / 3; i++) | ||
62 | { | ||
63 | int i1 = result.Indices[i * 3 + 0]; | ||
64 | int i2 = result.Indices[i * 3 + 1]; | ||
65 | int i3 = result.Indices[i * 3 + 2]; | ||
66 | |||
67 | float3 p1 = result.OutputVertices[i1]; | ||
68 | float3 p2 = result.OutputVertices[i2]; | ||
69 | float3 p3 = result.OutputVertices[i3]; | ||
70 | |||
71 | CTri t = new CTri(p1, p2, p3, i1, i2, i3); | ||
72 | tris.Add(t); | ||
73 | } | ||
74 | |||
75 | // we have not pre-computed the plane equation for each triangle in the convex hull.. | ||
76 | float totalVolume = 0; | ||
77 | |||
78 | List<CTri> ftris = new List<CTri>(); // 'feature' triangles. | ||
79 | List<CTri> input_mesh = new List<CTri>(); | ||
80 | |||
81 | for (int i = 0; i < indices.Count / 3; i++) | ||
82 | { | ||
83 | int i1 = indices[i * 3 + 0]; | ||
84 | int i2 = indices[i * 3 + 1]; | ||
85 | int i3 = indices[i * 3 + 2]; | ||
86 | |||
87 | float3 p1 = vertices[i1]; | ||
88 | float3 p2 = vertices[i2]; | ||
89 | float3 p3 = vertices[i3]; | ||
90 | |||
91 | CTri t = new CTri(p1, p2, p3, i1, i2, i3); | ||
92 | input_mesh.Add(t); | ||
93 | } | ||
94 | |||
95 | for (int i = 0; i < indices.Count / 3; i++) | ||
96 | { | ||
97 | int i1 = indices[i * 3 + 0]; | ||
98 | int i2 = indices[i * 3 + 1]; | ||
99 | int i3 = indices[i * 3 + 2]; | ||
100 | |||
101 | float3 p1 = vertices[i1]; | ||
102 | float3 p2 = vertices[i2]; | ||
103 | float3 p3 = vertices[i3]; | ||
104 | |||
105 | CTri t = new CTri(p1, p2, p3, i1, i2, i3); | ||
106 | |||
107 | featureMatch(t, tris, input_mesh); | ||
108 | |||
109 | if (t.mConcavity > 0.05f) | ||
110 | { | ||
111 | float v = t.getVolume(); | ||
112 | totalVolume += v; | ||
113 | ftris.Add(t); | ||
114 | } | ||
115 | } | ||
116 | |||
117 | SplitPlane.computeSplitPlane(vertices, indices, ref plane); | ||
118 | cret = totalVolume; | ||
119 | } | ||
120 | |||
121 | return cret; | ||
122 | } | ||
123 | |||
124 | public static bool featureMatch(CTri m, List<CTri> tris, List<CTri> input_mesh) | ||
125 | { | ||
126 | bool ret = false; | ||
127 | float neardot = 0.707f; | ||
128 | m.mConcavity = 0; | ||
129 | |||
130 | for (int i = 0; i < tris.Count; i++) | ||
131 | { | ||
132 | CTri t = tris[i]; | ||
133 | |||
134 | if (t.samePlane(m)) | ||
135 | { | ||
136 | ret = false; | ||
137 | break; | ||
138 | } | ||
139 | |||
140 | float dot = float3.dot(t.mNormal, m.mNormal); | ||
141 | |||
142 | if (dot > neardot) | ||
143 | { | ||
144 | float d1 = t.planeDistance(m.mP1); | ||
145 | float d2 = t.planeDistance(m.mP2); | ||
146 | float d3 = t.planeDistance(m.mP3); | ||
147 | |||
148 | if (d1 > 0.001f || d2 > 0.001f || d3 > 0.001f) // can't be near coplaner! | ||
149 | { | ||
150 | neardot = dot; | ||
151 | |||
152 | t.raySect(m.mP1, m.mNormal, ref m.mNear1); | ||
153 | t.raySect(m.mP2, m.mNormal, ref m.mNear2); | ||
154 | t.raySect(m.mP3, m.mNormal, ref m.mNear3); | ||
155 | |||
156 | ret = true; | ||
157 | } | ||
158 | } | ||
159 | } | ||
160 | |||
161 | if (ret) | ||
162 | { | ||
163 | m.mC1 = m.mP1.Distance(m.mNear1); | ||
164 | m.mC2 = m.mP2.Distance(m.mNear2); | ||
165 | m.mC3 = m.mP3.Distance(m.mNear3); | ||
166 | |||
167 | m.mConcavity = m.mC1; | ||
168 | |||
169 | if (m.mC2 > m.mConcavity) | ||
170 | m.mConcavity = m.mC2; | ||
171 | if (m.mC3 > m.mConcavity) | ||
172 | m.mConcavity = m.mC3; | ||
173 | } | ||
174 | |||
175 | return ret; | ||
176 | } | ||
177 | |||
178 | private static float det(float3 p1, float3 p2, float3 p3) | ||
179 | { | ||
180 | return p1.x * p2.y * p3.z + p2.x * p3.y * p1.z + p3.x * p1.y * p2.z - p1.x * p3.y * p2.z - p2.x * p1.y * p3.z - p3.x * p2.y * p1.z; | ||
181 | } | ||
182 | |||
183 | public static float computeMeshVolume(List<float3> vertices, List<int> indices) | ||
184 | { | ||
185 | float volume = 0f; | ||
186 | |||
187 | for (int i = 0; i < indices.Count / 3; i++) | ||
188 | { | ||
189 | float3 p1 = vertices[indices[i * 3 + 0]]; | ||
190 | float3 p2 = vertices[indices[i * 3 + 1]]; | ||
191 | float3 p3 = vertices[indices[i * 3 + 2]]; | ||
192 | |||
193 | volume += det(p1, p2, p3); // compute the volume of the tetrahedran relative to the origin. | ||
194 | } | ||
195 | |||
196 | volume *= (1.0f / 6.0f); | ||
197 | if (volume < 0f) | ||
198 | return -volume; | ||
199 | return volume; | ||
200 | } | ||
201 | |||
202 | public static float computeMeshVolume2(List<float3> vertices, List<int> indices) | ||
203 | { | ||
204 | float volume = 0f; | ||
205 | |||
206 | float3 p0 = vertices[0]; | ||
207 | for (int i = 0; i < indices.Count / 3; i++) | ||
208 | { | ||
209 | float3 p1 = vertices[indices[i * 3 + 0]]; | ||
210 | float3 p2 = vertices[indices[i * 3 + 1]]; | ||
211 | float3 p3 = vertices[indices[i * 3 + 2]]; | ||
212 | |||
213 | volume += tetVolume(p0, p1, p2, p3); // compute the volume of the tetrahedron relative to the root vertice | ||
214 | } | ||
215 | |||
216 | return volume * (1.0f / 6.0f); | ||
217 | } | ||
218 | |||
219 | private static float tetVolume(float3 p0, float3 p1, float3 p2, float3 p3) | ||
220 | { | ||
221 | float3 a = p1 - p0; | ||
222 | float3 b = p2 - p0; | ||
223 | float3 c = p3 - p0; | ||
224 | |||
225 | float3 cross = float3.cross(b, c); | ||
226 | float volume = float3.dot(a, cross); | ||
227 | |||
228 | if (volume < 0f) | ||
229 | return -volume; | ||
230 | return volume; | ||
231 | } | ||
232 | } | ||
233 | } | ||
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/ConvexBuilder.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/ConvexBuilder.cs new file mode 100644 index 0000000..dfaede1 --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/ConvexBuilder.cs | |||
@@ -0,0 +1,411 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | using System.Collections.Generic; | ||
30 | using System.Diagnostics; | ||
31 | |||
32 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
33 | { | ||
34 | public class DecompDesc | ||
35 | { | ||
36 | public List<float3> mVertices; | ||
37 | public List<int> mIndices; | ||
38 | |||
39 | // options | ||
40 | public uint mDepth; // depth to split, a maximum of 10, generally not over 7. | ||
41 | public float mCpercent; // the concavity threshold percentage. 0=20 is reasonable. | ||
42 | public float mPpercent; // the percentage volume conservation threshold to collapse hulls. 0-30 is reasonable. | ||
43 | |||
44 | // hull output limits. | ||
45 | public uint mMaxVertices; // maximum number of vertices in the output hull. Recommended 32 or less. | ||
46 | public float mSkinWidth; // a skin width to apply to the output hulls. | ||
47 | |||
48 | public ConvexDecompositionCallback mCallback; // the interface to receive back the results. | ||
49 | |||
50 | public DecompDesc() | ||
51 | { | ||
52 | mDepth = 5; | ||
53 | mCpercent = 5; | ||
54 | mPpercent = 5; | ||
55 | mMaxVertices = 32; | ||
56 | } | ||
57 | } | ||
58 | |||
59 | public class CHull | ||
60 | { | ||
61 | public float[] mMin = new float[3]; | ||
62 | public float[] mMax = new float[3]; | ||
63 | public float mVolume; | ||
64 | public float mDiagonal; | ||
65 | public ConvexResult mResult; | ||
66 | |||
67 | public CHull(ConvexResult result) | ||
68 | { | ||
69 | mResult = new ConvexResult(result); | ||
70 | mVolume = Concavity.computeMeshVolume(result.HullVertices, result.HullIndices); | ||
71 | |||
72 | mDiagonal = getBoundingRegion(result.HullVertices, mMin, mMax); | ||
73 | |||
74 | float dx = mMax[0] - mMin[0]; | ||
75 | float dy = mMax[1] - mMin[1]; | ||
76 | float dz = mMax[2] - mMin[2]; | ||
77 | |||
78 | dx *= 0.1f; // inflate 1/10th on each edge | ||
79 | dy *= 0.1f; // inflate 1/10th on each edge | ||
80 | dz *= 0.1f; // inflate 1/10th on each edge | ||
81 | |||
82 | mMin[0] -= dx; | ||
83 | mMin[1] -= dy; | ||
84 | mMin[2] -= dz; | ||
85 | |||
86 | mMax[0] += dx; | ||
87 | mMax[1] += dy; | ||
88 | mMax[2] += dz; | ||
89 | } | ||
90 | |||
91 | public void Dispose() | ||
92 | { | ||
93 | mResult = null; | ||
94 | } | ||
95 | |||
96 | public bool overlap(CHull h) | ||
97 | { | ||
98 | return overlapAABB(mMin, mMax, h.mMin, h.mMax); | ||
99 | } | ||
100 | |||
101 | // returns the d1Giagonal distance | ||
102 | private static float getBoundingRegion(List<float3> points, float[] bmin, float[] bmax) | ||
103 | { | ||
104 | float3 first = points[0]; | ||
105 | |||
106 | bmin[0] = first.x; | ||
107 | bmin[1] = first.y; | ||
108 | bmin[2] = first.z; | ||
109 | |||
110 | bmax[0] = first.x; | ||
111 | bmax[1] = first.y; | ||
112 | bmax[2] = first.z; | ||
113 | |||
114 | for (int i = 1; i < points.Count; i++) | ||
115 | { | ||
116 | float3 p = points[i]; | ||
117 | |||
118 | if (p[0] < bmin[0]) bmin[0] = p[0]; | ||
119 | if (p[1] < bmin[1]) bmin[1] = p[1]; | ||
120 | if (p[2] < bmin[2]) bmin[2] = p[2]; | ||
121 | |||
122 | if (p[0] > bmax[0]) bmax[0] = p[0]; | ||
123 | if (p[1] > bmax[1]) bmax[1] = p[1]; | ||
124 | if (p[2] > bmax[2]) bmax[2] = p[2]; | ||
125 | } | ||
126 | |||
127 | float dx = bmax[0] - bmin[0]; | ||
128 | float dy = bmax[1] - bmin[1]; | ||
129 | float dz = bmax[2] - bmin[2]; | ||
130 | |||
131 | return (float)Math.Sqrt(dx * dx + dy * dy + dz * dz); | ||
132 | } | ||
133 | |||
134 | // return true if the two AABB's overlap. | ||
135 | private static bool overlapAABB(float[] bmin1, float[] bmax1, float[] bmin2, float[] bmax2) | ||
136 | { | ||
137 | if (bmax2[0] < bmin1[0]) return false; // if the maximum is less than our minimum on any axis | ||
138 | if (bmax2[1] < bmin1[1]) return false; | ||
139 | if (bmax2[2] < bmin1[2]) return false; | ||
140 | |||
141 | if (bmin2[0] > bmax1[0]) return false; // if the minimum is greater than our maximum on any axis | ||
142 | if (bmin2[1] > bmax1[1]) return false; // if the minimum is greater than our maximum on any axis | ||
143 | if (bmin2[2] > bmax1[2]) return false; // if the minimum is greater than our maximum on any axis | ||
144 | |||
145 | return true; // the extents overlap | ||
146 | } | ||
147 | } | ||
148 | |||
149 | public class ConvexBuilder | ||
150 | { | ||
151 | public List<CHull> mChulls = new List<CHull>(); | ||
152 | private ConvexDecompositionCallback mCallback; | ||
153 | |||
154 | private int MAXDEPTH = 8; | ||
155 | private float CONCAVE_PERCENT = 1f; | ||
156 | private float MERGE_PERCENT = 2f; | ||
157 | |||
158 | public ConvexBuilder(ConvexDecompositionCallback callback) | ||
159 | { | ||
160 | mCallback = callback; | ||
161 | } | ||
162 | |||
163 | public void Dispose() | ||
164 | { | ||
165 | int i; | ||
166 | for (i = 0; i < mChulls.Count; i++) | ||
167 | { | ||
168 | CHull cr = mChulls[i]; | ||
169 | cr.Dispose(); | ||
170 | } | ||
171 | } | ||
172 | |||
173 | public bool isDuplicate(uint i1, uint i2, uint i3, uint ci1, uint ci2, uint ci3) | ||
174 | { | ||
175 | uint dcount = 0; | ||
176 | |||
177 | Debug.Assert(i1 != i2 && i1 != i3 && i2 != i3); | ||
178 | Debug.Assert(ci1 != ci2 && ci1 != ci3 && ci2 != ci3); | ||
179 | |||
180 | if (i1 == ci1 || i1 == ci2 || i1 == ci3) | ||
181 | dcount++; | ||
182 | if (i2 == ci1 || i2 == ci2 || i2 == ci3) | ||
183 | dcount++; | ||
184 | if (i3 == ci1 || i3 == ci2 || i3 == ci3) | ||
185 | dcount++; | ||
186 | |||
187 | return dcount == 3; | ||
188 | } | ||
189 | |||
190 | public void getMesh(ConvexResult cr, VertexPool vc, List<int> indices) | ||
191 | { | ||
192 | List<int> src = cr.HullIndices; | ||
193 | |||
194 | for (int i = 0; i < src.Count / 3; i++) | ||
195 | { | ||
196 | int i1 = src[i * 3 + 0]; | ||
197 | int i2 = src[i * 3 + 1]; | ||
198 | int i3 = src[i * 3 + 2]; | ||
199 | |||
200 | float3 p1 = cr.HullVertices[i1]; | ||
201 | float3 p2 = cr.HullVertices[i2]; | ||
202 | float3 p3 = cr.HullVertices[i3]; | ||
203 | |||
204 | i1 = vc.getIndex(p1); | ||
205 | i2 = vc.getIndex(p2); | ||
206 | i3 = vc.getIndex(p3); | ||
207 | } | ||
208 | } | ||
209 | |||
210 | public CHull canMerge(CHull a, CHull b) | ||
211 | { | ||
212 | if (!a.overlap(b)) // if their AABB's (with a little slop) don't overlap, then return. | ||
213 | return null; | ||
214 | |||
215 | CHull ret = null; | ||
216 | |||
217 | // ok..we are going to combine both meshes into a single mesh | ||
218 | // and then we are going to compute the concavity... | ||
219 | |||
220 | VertexPool vc = new VertexPool(); | ||
221 | |||
222 | List<int> indices = new List<int>(); | ||
223 | |||
224 | getMesh(a.mResult, vc, indices); | ||
225 | getMesh(b.mResult, vc, indices); | ||
226 | |||
227 | int vcount = vc.GetSize(); | ||
228 | List<float3> vertices = vc.GetVertices(); | ||
229 | int tcount = indices.Count / 3; | ||
230 | |||
231 | //don't do anything if hull is empty | ||
232 | if (tcount == 0) | ||
233 | { | ||
234 | vc.Clear(); | ||
235 | return null; | ||
236 | } | ||
237 | |||
238 | HullResult hresult = new HullResult(); | ||
239 | HullDesc desc = new HullDesc(); | ||
240 | |||
241 | desc.SetHullFlag(HullFlag.QF_TRIANGLES); | ||
242 | desc.Vertices = vertices; | ||
243 | |||
244 | HullError hret = HullUtils.CreateConvexHull(desc, ref hresult); | ||
245 | |||
246 | if (hret == HullError.QE_OK) | ||
247 | { | ||
248 | float combineVolume = Concavity.computeMeshVolume(hresult.OutputVertices, hresult.Indices); | ||
249 | float sumVolume = a.mVolume + b.mVolume; | ||
250 | |||
251 | float percent = (sumVolume * 100) / combineVolume; | ||
252 | if (percent >= (100.0f - MERGE_PERCENT)) | ||
253 | { | ||
254 | ConvexResult cr = new ConvexResult(hresult.OutputVertices, hresult.Indices); | ||
255 | ret = new CHull(cr); | ||
256 | } | ||
257 | } | ||
258 | |||
259 | vc.Clear(); | ||
260 | return ret; | ||
261 | } | ||
262 | |||
263 | public bool combineHulls() | ||
264 | { | ||
265 | bool combine = false; | ||
266 | |||
267 | sortChulls(mChulls); // sort the convex hulls, largest volume to least... | ||
268 | |||
269 | List<CHull> output = new List<CHull>(); // the output hulls... | ||
270 | |||
271 | int i; | ||
272 | for (i = 0; i < mChulls.Count && !combine; ++i) | ||
273 | { | ||
274 | CHull cr = mChulls[i]; | ||
275 | |||
276 | int j; | ||
277 | for (j = 0; j < mChulls.Count; j++) | ||
278 | { | ||
279 | CHull match = mChulls[j]; | ||
280 | |||
281 | if (cr != match) // don't try to merge a hull with itself, that be stoopid | ||
282 | { | ||
283 | |||
284 | CHull merge = canMerge(cr, match); // if we can merge these two.... | ||
285 | |||
286 | if (merge != null) | ||
287 | { | ||
288 | output.Add(merge); | ||
289 | |||
290 | ++i; | ||
291 | while (i != mChulls.Count) | ||
292 | { | ||
293 | CHull cr2 = mChulls[i]; | ||
294 | if (cr2 != match) | ||
295 | { | ||
296 | output.Add(cr2); | ||
297 | } | ||
298 | i++; | ||
299 | } | ||
300 | |||
301 | cr.Dispose(); | ||
302 | match.Dispose(); | ||
303 | combine = true; | ||
304 | break; | ||
305 | } | ||
306 | } | ||
307 | } | ||
308 | |||
309 | if (combine) | ||
310 | { | ||
311 | break; | ||
312 | } | ||
313 | else | ||
314 | { | ||
315 | output.Add(cr); | ||
316 | } | ||
317 | } | ||
318 | |||
319 | if (combine) | ||
320 | { | ||
321 | mChulls.Clear(); | ||
322 | mChulls = output; | ||
323 | output.Clear(); | ||
324 | } | ||
325 | |||
326 | return combine; | ||
327 | } | ||
328 | |||
329 | public int process(DecompDesc desc) | ||
330 | { | ||
331 | int ret = 0; | ||
332 | |||
333 | MAXDEPTH = (int)desc.mDepth; | ||
334 | CONCAVE_PERCENT = desc.mCpercent; | ||
335 | MERGE_PERCENT = desc.mPpercent; | ||
336 | |||
337 | ConvexDecomposition.calcConvexDecomposition(desc.mVertices, desc.mIndices, ConvexDecompResult, 0f, 0, MAXDEPTH, CONCAVE_PERCENT, MERGE_PERCENT); | ||
338 | |||
339 | while (combineHulls()) // keep combinging hulls until I can't combine any more... | ||
340 | ; | ||
341 | |||
342 | int i; | ||
343 | for (i = 0; i < mChulls.Count; i++) | ||
344 | { | ||
345 | CHull cr = mChulls[i]; | ||
346 | |||
347 | // before we hand it back to the application, we need to regenerate the hull based on the | ||
348 | // limits given by the user. | ||
349 | |||
350 | ConvexResult c = cr.mResult; // the high resolution hull... | ||
351 | |||
352 | HullResult result = new HullResult(); | ||
353 | HullDesc hdesc = new HullDesc(); | ||
354 | |||
355 | hdesc.SetHullFlag(HullFlag.QF_TRIANGLES); | ||
356 | |||
357 | hdesc.Vertices = c.HullVertices; | ||
358 | hdesc.MaxVertices = desc.mMaxVertices; // maximum number of vertices allowed in the output | ||
359 | |||
360 | if (desc.mSkinWidth != 0f) | ||
361 | { | ||
362 | hdesc.SkinWidth = desc.mSkinWidth; | ||
363 | hdesc.SetHullFlag(HullFlag.QF_SKIN_WIDTH); // do skin width computation. | ||
364 | } | ||
365 | |||
366 | HullError ret2 = HullUtils.CreateConvexHull(hdesc, ref result); | ||
367 | |||
368 | if (ret2 == HullError.QE_OK) | ||
369 | { | ||
370 | ConvexResult r = new ConvexResult(result.OutputVertices, result.Indices); | ||
371 | |||
372 | r.mHullVolume = Concavity.computeMeshVolume(result.OutputVertices, result.Indices); // the volume of the hull. | ||
373 | |||
374 | // compute the best fit OBB | ||
375 | //computeBestFitOBB(result.mNumOutputVertices, result.mOutputVertices, sizeof(float) * 3, r.mOBBSides, r.mOBBTransform); | ||
376 | |||
377 | //r.mOBBVolume = r.mOBBSides[0] * r.mOBBSides[1] * r.mOBBSides[2]; // compute the OBB volume. | ||
378 | |||
379 | //fm_getTranslation(r.mOBBTransform, r.mOBBCenter); // get the translation component of the 4x4 matrix. | ||
380 | |||
381 | //fm_matrixToQuat(r.mOBBTransform, r.mOBBOrientation); // extract the orientation as a quaternion. | ||
382 | |||
383 | //r.mSphereRadius = computeBoundingSphere(result.mNumOutputVertices, result.mOutputVertices, r.mSphereCenter); | ||
384 | //r.mSphereVolume = fm_sphereVolume(r.mSphereRadius); | ||
385 | |||
386 | mCallback(r); | ||
387 | } | ||
388 | |||
389 | result = null; | ||
390 | cr.Dispose(); | ||
391 | } | ||
392 | |||
393 | ret = mChulls.Count; | ||
394 | |||
395 | mChulls.Clear(); | ||
396 | |||
397 | return ret; | ||
398 | } | ||
399 | |||
400 | public void ConvexDecompResult(ConvexResult result) | ||
401 | { | ||
402 | CHull ch = new CHull(result); | ||
403 | mChulls.Add(ch); | ||
404 | } | ||
405 | |||
406 | public void sortChulls(List<CHull> hulls) | ||
407 | { | ||
408 | hulls.Sort(delegate(CHull a, CHull b) { return a.mVolume.CompareTo(b.mVolume); }); | ||
409 | } | ||
410 | } | ||
411 | } | ||
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/ConvexDecomposition.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/ConvexDecomposition.cs new file mode 100644 index 0000000..2e2bb70 --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/ConvexDecomposition.cs | |||
@@ -0,0 +1,200 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | using System.Collections.Generic; | ||
30 | using System.Diagnostics; | ||
31 | |||
32 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
33 | { | ||
34 | public delegate void ConvexDecompositionCallback(ConvexResult result); | ||
35 | |||
36 | public class FaceTri | ||
37 | { | ||
38 | public float3 P1; | ||
39 | public float3 P2; | ||
40 | public float3 P3; | ||
41 | |||
42 | public FaceTri() { } | ||
43 | |||
44 | public FaceTri(List<float3> vertices, int i1, int i2, int i3) | ||
45 | { | ||
46 | P1 = new float3(vertices[i1]); | ||
47 | P2 = new float3(vertices[i2]); | ||
48 | P3 = new float3(vertices[i3]); | ||
49 | } | ||
50 | } | ||
51 | |||
52 | public static class ConvexDecomposition | ||
53 | { | ||
54 | private static void addTri(VertexPool vl, List<int> list, float3 p1, float3 p2, float3 p3) | ||
55 | { | ||
56 | int i1 = vl.getIndex(p1); | ||
57 | int i2 = vl.getIndex(p2); | ||
58 | int i3 = vl.getIndex(p3); | ||
59 | |||
60 | // do *not* process degenerate triangles! | ||
61 | if ( i1 != i2 && i1 != i3 && i2 != i3 ) | ||
62 | { | ||
63 | list.Add(i1); | ||
64 | list.Add(i2); | ||
65 | list.Add(i3); | ||
66 | } | ||
67 | } | ||
68 | |||
69 | public static void calcConvexDecomposition(List<float3> vertices, List<int> indices, ConvexDecompositionCallback callback, float masterVolume, int depth, | ||
70 | int maxDepth, float concavePercent, float mergePercent) | ||
71 | { | ||
72 | float4 plane = new float4(); | ||
73 | bool split = false; | ||
74 | |||
75 | if (depth < maxDepth) | ||
76 | { | ||
77 | float volume = 0f; | ||
78 | float c = Concavity.computeConcavity(vertices, indices, ref plane, ref volume); | ||
79 | |||
80 | if (depth == 0) | ||
81 | { | ||
82 | masterVolume = volume; | ||
83 | } | ||
84 | |||
85 | float percent = (c * 100.0f) / masterVolume; | ||
86 | |||
87 | if (percent > concavePercent) // if great than 5% of the total volume is concave, go ahead and keep splitting. | ||
88 | { | ||
89 | split = true; | ||
90 | } | ||
91 | } | ||
92 | |||
93 | if (depth >= maxDepth || !split) | ||
94 | { | ||
95 | HullResult result = new HullResult(); | ||
96 | HullDesc desc = new HullDesc(); | ||
97 | |||
98 | desc.SetHullFlag(HullFlag.QF_TRIANGLES); | ||
99 | |||
100 | desc.Vertices = vertices; | ||
101 | |||
102 | HullError ret = HullUtils.CreateConvexHull(desc, ref result); | ||
103 | |||
104 | if (ret == HullError.QE_OK) | ||
105 | { | ||
106 | ConvexResult r = new ConvexResult(result.OutputVertices, result.Indices); | ||
107 | callback(r); | ||
108 | } | ||
109 | |||
110 | return; | ||
111 | } | ||
112 | |||
113 | List<int> ifront = new List<int>(); | ||
114 | List<int> iback = new List<int>(); | ||
115 | |||
116 | VertexPool vfront = new VertexPool(); | ||
117 | VertexPool vback = new VertexPool(); | ||
118 | |||
119 | // ok..now we are going to 'split' all of the input triangles against this plane! | ||
120 | for (int i = 0; i < indices.Count / 3; i++) | ||
121 | { | ||
122 | int i1 = indices[i * 3 + 0]; | ||
123 | int i2 = indices[i * 3 + 1]; | ||
124 | int i3 = indices[i * 3 + 2]; | ||
125 | |||
126 | FaceTri t = new FaceTri(vertices, i1, i2, i3); | ||
127 | |||
128 | float3[] front = new float3[4]; | ||
129 | float3[] back = new float3[4]; | ||
130 | |||
131 | int fcount = 0; | ||
132 | int bcount = 0; | ||
133 | |||
134 | PlaneTriResult result = PlaneTri.planeTriIntersection(plane, t, 0.00001f, ref front, out fcount, ref back, out bcount); | ||
135 | |||
136 | if (fcount > 4 || bcount > 4) | ||
137 | { | ||
138 | result = PlaneTri.planeTriIntersection(plane, t, 0.00001f, ref front, out fcount, ref back, out bcount); | ||
139 | } | ||
140 | |||
141 | switch (result) | ||
142 | { | ||
143 | case PlaneTriResult.PTR_FRONT: | ||
144 | Debug.Assert(fcount == 3); | ||
145 | addTri(vfront, ifront, front[0], front[1], front[2]); | ||
146 | break; | ||
147 | case PlaneTriResult.PTR_BACK: | ||
148 | Debug.Assert(bcount == 3); | ||
149 | addTri(vback, iback, back[0], back[1], back[2]); | ||
150 | break; | ||
151 | case PlaneTriResult.PTR_SPLIT: | ||
152 | Debug.Assert(fcount >= 3 && fcount <= 4); | ||
153 | Debug.Assert(bcount >= 3 && bcount <= 4); | ||
154 | |||
155 | addTri(vfront, ifront, front[0], front[1], front[2]); | ||
156 | addTri(vback, iback, back[0], back[1], back[2]); | ||
157 | |||
158 | if (fcount == 4) | ||
159 | { | ||
160 | addTri(vfront, ifront, front[0], front[2], front[3]); | ||
161 | } | ||
162 | |||
163 | if (bcount == 4) | ||
164 | { | ||
165 | addTri(vback, iback, back[0], back[2], back[3]); | ||
166 | } | ||
167 | |||
168 | break; | ||
169 | } | ||
170 | } | ||
171 | |||
172 | // ok... here we recursively call | ||
173 | if (ifront.Count > 0) | ||
174 | { | ||
175 | int vcount = vfront.GetSize(); | ||
176 | List<float3> vertices2 = vfront.GetVertices(); | ||
177 | for (int i = 0; i < vertices2.Count; i++) | ||
178 | vertices2[i] = new float3(vertices2[i]); | ||
179 | int tcount = ifront.Count / 3; | ||
180 | |||
181 | calcConvexDecomposition(vertices2, ifront, callback, masterVolume, depth + 1, maxDepth, concavePercent, mergePercent); | ||
182 | } | ||
183 | |||
184 | ifront.Clear(); | ||
185 | vfront.Clear(); | ||
186 | |||
187 | if (iback.Count > 0) | ||
188 | { | ||
189 | int vcount = vback.GetSize(); | ||
190 | List<float3> vertices2 = vback.GetVertices(); | ||
191 | int tcount = iback.Count / 3; | ||
192 | |||
193 | calcConvexDecomposition(vertices2, iback, callback, masterVolume, depth + 1, maxDepth, concavePercent, mergePercent); | ||
194 | } | ||
195 | |||
196 | iback.Clear(); | ||
197 | vback.Clear(); | ||
198 | } | ||
199 | } | ||
200 | } | ||
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/ConvexResult.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/ConvexResult.cs new file mode 100644 index 0000000..87758b5 --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/ConvexResult.cs | |||
@@ -0,0 +1,74 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | using System.Collections.Generic; | ||
30 | |||
31 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
32 | { | ||
33 | public class ConvexResult | ||
34 | { | ||
35 | public List<float3> HullVertices; | ||
36 | public List<int> HullIndices; | ||
37 | |||
38 | public float mHullVolume; // the volume of the convex hull. | ||
39 | |||
40 | //public float[] OBBSides = new float[3]; // the width, height and breadth of the best fit OBB | ||
41 | //public float[] OBBCenter = new float[3]; // the center of the OBB | ||
42 | //public float[] OBBOrientation = new float[4]; // the quaternion rotation of the OBB. | ||
43 | //public float[] OBBTransform = new float[16]; // the 4x4 transform of the OBB. | ||
44 | //public float OBBVolume; // the volume of the OBB | ||
45 | |||
46 | //public float SphereRadius; // radius and center of best fit sphere | ||
47 | //public float[] SphereCenter = new float[3]; | ||
48 | //public float SphereVolume; // volume of the best fit sphere | ||
49 | |||
50 | public ConvexResult() | ||
51 | { | ||
52 | HullVertices = new List<float3>(); | ||
53 | HullIndices = new List<int>(); | ||
54 | } | ||
55 | |||
56 | public ConvexResult(List<float3> hvertices, List<int> hindices) | ||
57 | { | ||
58 | HullVertices = hvertices; | ||
59 | HullIndices = hindices; | ||
60 | } | ||
61 | |||
62 | public ConvexResult(ConvexResult r) | ||
63 | { | ||
64 | HullVertices = new List<float3>(r.HullVertices); | ||
65 | HullIndices = new List<int>(r.HullIndices); | ||
66 | } | ||
67 | |||
68 | public void Dispose() | ||
69 | { | ||
70 | HullVertices = null; | ||
71 | HullIndices = null; | ||
72 | } | ||
73 | } | ||
74 | } | ||
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/HullClasses.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/HullClasses.cs new file mode 100644 index 0000000..d81df26 --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/HullClasses.cs | |||
@@ -0,0 +1,171 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | using System.Collections.Generic; | ||
30 | |||
31 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
32 | { | ||
33 | public class HullResult | ||
34 | { | ||
35 | public bool Polygons = true; // true if indices represents polygons, false indices are triangles | ||
36 | public List<float3> OutputVertices = new List<float3>(); | ||
37 | public List<int> Indices; | ||
38 | |||
39 | // If triangles, then indices are array indexes into the vertex list. | ||
40 | // If polygons, indices are in the form (number of points in face) (p1, p2, p3, ..) etc.. | ||
41 | } | ||
42 | |||
43 | public class PHullResult | ||
44 | { | ||
45 | public List<float3> Vertices = new List<float3>(); | ||
46 | public List<int> Indices = new List<int>(); | ||
47 | } | ||
48 | |||
49 | [Flags] | ||
50 | public enum HullFlag : int | ||
51 | { | ||
52 | QF_DEFAULT = 0, | ||
53 | QF_TRIANGLES = (1 << 0), // report results as triangles, not polygons. | ||
54 | QF_SKIN_WIDTH = (1 << 2) // extrude hull based on this skin width | ||
55 | } | ||
56 | |||
57 | public enum HullError : int | ||
58 | { | ||
59 | QE_OK, // success! | ||
60 | QE_FAIL // failed. | ||
61 | } | ||
62 | |||
63 | public class HullDesc | ||
64 | { | ||
65 | public HullFlag Flags; // flags to use when generating the convex hull. | ||
66 | public List<float3> Vertices; | ||
67 | public float NormalEpsilon; // the epsilon for removing duplicates. This is a normalized value, if normalized bit is on. | ||
68 | public float SkinWidth; | ||
69 | public uint MaxVertices; // maximum number of vertices to be considered for the hull! | ||
70 | public uint MaxFaces; | ||
71 | |||
72 | public HullDesc() | ||
73 | { | ||
74 | Flags = HullFlag.QF_DEFAULT; | ||
75 | Vertices = new List<float3>(); | ||
76 | NormalEpsilon = 0.001f; | ||
77 | MaxVertices = 4096; | ||
78 | MaxFaces = 4096; | ||
79 | SkinWidth = 0.01f; | ||
80 | } | ||
81 | |||
82 | public HullDesc(HullFlag flags, List<float3> vertices) | ||
83 | { | ||
84 | Flags = flags; | ||
85 | Vertices = new List<float3>(vertices); | ||
86 | NormalEpsilon = 0.001f; | ||
87 | MaxVertices = 4096; | ||
88 | MaxFaces = 4096; | ||
89 | SkinWidth = 0.01f; | ||
90 | } | ||
91 | |||
92 | public bool HasHullFlag(HullFlag flag) | ||
93 | { | ||
94 | return (Flags & flag) != 0; | ||
95 | } | ||
96 | |||
97 | public void SetHullFlag(HullFlag flag) | ||
98 | { | ||
99 | Flags |= flag; | ||
100 | } | ||
101 | |||
102 | public void ClearHullFlag(HullFlag flag) | ||
103 | { | ||
104 | Flags &= ~flag; | ||
105 | } | ||
106 | } | ||
107 | |||
108 | public class ConvexH | ||
109 | { | ||
110 | public struct HalfEdge | ||
111 | { | ||
112 | public short ea; // the other half of the edge (index into edges list) | ||
113 | public byte v; // the vertex at the start of this edge (index into vertices list) | ||
114 | public byte p; // the facet on which this edge lies (index into facets list) | ||
115 | |||
116 | public HalfEdge(short _ea, byte _v, byte _p) | ||
117 | { | ||
118 | ea = _ea; | ||
119 | v = _v; | ||
120 | p = _p; | ||
121 | } | ||
122 | |||
123 | public HalfEdge(HalfEdge e) | ||
124 | { | ||
125 | ea = e.ea; | ||
126 | v = e.v; | ||
127 | p = e.p; | ||
128 | } | ||
129 | } | ||
130 | |||
131 | public List<float3> vertices = new List<float3>(); | ||
132 | public List<HalfEdge> edges = new List<HalfEdge>(); | ||
133 | public List<Plane> facets = new List<Plane>(); | ||
134 | |||
135 | public ConvexH(int vertices_size, int edges_size, int facets_size) | ||
136 | { | ||
137 | vertices = new List<float3>(vertices_size); | ||
138 | edges = new List<HalfEdge>(edges_size); | ||
139 | facets = new List<Plane>(facets_size); | ||
140 | } | ||
141 | } | ||
142 | |||
143 | public class VertFlag | ||
144 | { | ||
145 | public byte planetest; | ||
146 | public byte junk; | ||
147 | public byte undermap; | ||
148 | public byte overmap; | ||
149 | } | ||
150 | |||
151 | public class EdgeFlag | ||
152 | { | ||
153 | public byte planetest; | ||
154 | public byte fixes; | ||
155 | public short undermap; | ||
156 | public short overmap; | ||
157 | } | ||
158 | |||
159 | public class PlaneFlag | ||
160 | { | ||
161 | public byte undermap; | ||
162 | public byte overmap; | ||
163 | } | ||
164 | |||
165 | public class Coplanar | ||
166 | { | ||
167 | public ushort ea; | ||
168 | public byte v0; | ||
169 | public byte v1; | ||
170 | } | ||
171 | } | ||
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/HullTriangle.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/HullTriangle.cs new file mode 100644 index 0000000..1119a75 --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/HullTriangle.cs | |||
@@ -0,0 +1,99 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | using System.Collections.Generic; | ||
30 | using System.Diagnostics; | ||
31 | |||
32 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
33 | { | ||
34 | public class HullTriangle : int3 | ||
35 | { | ||
36 | public int3 n = new int3(); | ||
37 | public int id; | ||
38 | public int vmax; | ||
39 | public float rise; | ||
40 | private List<HullTriangle> tris; | ||
41 | |||
42 | public HullTriangle(int a, int b, int c, List<HullTriangle> tris) | ||
43 | : base(a, b, c) | ||
44 | { | ||
45 | this.tris = tris; | ||
46 | |||
47 | n = new int3(-1, -1, -1); | ||
48 | id = tris.Count; | ||
49 | tris.Add(this); | ||
50 | vmax = -1; | ||
51 | rise = 0.0f; | ||
52 | } | ||
53 | |||
54 | public void Dispose() | ||
55 | { | ||
56 | Debug.Assert(tris[id] == this); | ||
57 | tris[id] = null; | ||
58 | } | ||
59 | |||
60 | public int neib(int a, int b) | ||
61 | { | ||
62 | int i; | ||
63 | |||
64 | for (i = 0; i < 3; i++) | ||
65 | { | ||
66 | int i1 = (i + 1) % 3; | ||
67 | int i2 = (i + 2) % 3; | ||
68 | if ((this)[i] == a && (this)[i1] == b) | ||
69 | return n[i2]; | ||
70 | if ((this)[i] == b && (this)[i1] == a) | ||
71 | return n[i2]; | ||
72 | } | ||
73 | |||
74 | Debug.Assert(false); | ||
75 | return -1; | ||
76 | } | ||
77 | |||
78 | public void setneib(int a, int b, int value) | ||
79 | { | ||
80 | int i; | ||
81 | |||
82 | for (i = 0; i < 3; i++) | ||
83 | { | ||
84 | int i1 = (i + 1) % 3; | ||
85 | int i2 = (i + 2) % 3; | ||
86 | if ((this)[i] == a && (this)[i1] == b) | ||
87 | { | ||
88 | n[i2] = value; | ||
89 | return; | ||
90 | } | ||
91 | if ((this)[i] == b && (this)[i1] == a) | ||
92 | { | ||
93 | n[i2] = value; | ||
94 | return; | ||
95 | } | ||
96 | } | ||
97 | } | ||
98 | } | ||
99 | } | ||
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/HullUtils.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/HullUtils.cs new file mode 100644 index 0000000..c9ccfe2 --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/HullUtils.cs | |||
@@ -0,0 +1,1868 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | using System.Collections.Generic; | ||
30 | using System.Diagnostics; | ||
31 | |||
32 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
33 | { | ||
34 | public static class HullUtils | ||
35 | { | ||
36 | public static int argmin(float[] a, int n) | ||
37 | { | ||
38 | int r = 0; | ||
39 | for (int i = 1; i < n; i++) | ||
40 | { | ||
41 | if (a[i] < a[r]) | ||
42 | { | ||
43 | r = i; | ||
44 | } | ||
45 | } | ||
46 | return r; | ||
47 | } | ||
48 | |||
49 | public static float clampf(float a) | ||
50 | { | ||
51 | return Math.Min(1.0f, Math.Max(0.0f, a)); | ||
52 | } | ||
53 | |||
54 | public static float Round(float a, float precision) | ||
55 | { | ||
56 | return (float)Math.Floor(0.5f + a / precision) * precision; | ||
57 | } | ||
58 | |||
59 | public static float Interpolate(float f0, float f1, float alpha) | ||
60 | { | ||
61 | return f0 * (1 - alpha) + f1 * alpha; | ||
62 | } | ||
63 | |||
64 | public static void Swap<T>(ref T a, ref T b) | ||
65 | { | ||
66 | T tmp = a; | ||
67 | a = b; | ||
68 | b = tmp; | ||
69 | } | ||
70 | |||
71 | public static bool above(List<float3> vertices, int3 t, float3 p, float epsilon) | ||
72 | { | ||
73 | float3 vtx = vertices[t.x]; | ||
74 | float3 n = TriNormal(vtx, vertices[t.y], vertices[t.z]); | ||
75 | return (float3.dot(n, p - vtx) > epsilon); // EPSILON??? | ||
76 | } | ||
77 | |||
78 | public static int hasedge(int3 t, int a, int b) | ||
79 | { | ||
80 | for (int i = 0; i < 3; i++) | ||
81 | { | ||
82 | int i1 = (i + 1) % 3; | ||
83 | if (t[i] == a && t[i1] == b) | ||
84 | return 1; | ||
85 | } | ||
86 | return 0; | ||
87 | } | ||
88 | |||
89 | public static bool hasvert(int3 t, int v) | ||
90 | { | ||
91 | return (t[0] == v || t[1] == v || t[2] == v); | ||
92 | } | ||
93 | |||
94 | public static int shareedge(int3 a, int3 b) | ||
95 | { | ||
96 | int i; | ||
97 | for (i = 0; i < 3; i++) | ||
98 | { | ||
99 | int i1 = (i + 1) % 3; | ||
100 | if (hasedge(a, b[i1], b[i]) != 0) | ||
101 | return 1; | ||
102 | } | ||
103 | return 0; | ||
104 | } | ||
105 | |||
106 | public static void b2bfix(HullTriangle s, HullTriangle t, List<HullTriangle> tris) | ||
107 | { | ||
108 | int i; | ||
109 | for (i = 0; i < 3; i++) | ||
110 | { | ||
111 | int i1 = (i + 1) % 3; | ||
112 | int i2 = (i + 2) % 3; | ||
113 | int a = (s)[i1]; | ||
114 | int b = (s)[i2]; | ||
115 | Debug.Assert(tris[s.neib(a, b)].neib(b, a) == s.id); | ||
116 | Debug.Assert(tris[t.neib(a, b)].neib(b, a) == t.id); | ||
117 | tris[s.neib(a, b)].setneib(b, a, t.neib(b, a)); | ||
118 | tris[t.neib(b, a)].setneib(a, b, s.neib(a, b)); | ||
119 | } | ||
120 | } | ||
121 | |||
122 | public static void removeb2b(HullTriangle s, HullTriangle t, List<HullTriangle> tris) | ||
123 | { | ||
124 | b2bfix(s, t, tris); | ||
125 | s.Dispose(); | ||
126 | t.Dispose(); | ||
127 | } | ||
128 | |||
129 | public static void checkit(HullTriangle t, List<HullTriangle> tris) | ||
130 | { | ||
131 | int i; | ||
132 | Debug.Assert(tris[t.id] == t); | ||
133 | for (i = 0; i < 3; i++) | ||
134 | { | ||
135 | int i1 = (i + 1) % 3; | ||
136 | int i2 = (i + 2) % 3; | ||
137 | int a = (t)[i1]; | ||
138 | int b = (t)[i2]; | ||
139 | Debug.Assert(a != b); | ||
140 | Debug.Assert(tris[t.n[i]].neib(b, a) == t.id); | ||
141 | } | ||
142 | } | ||
143 | |||
144 | public static void extrude(HullTriangle t0, int v, List<HullTriangle> tris) | ||
145 | { | ||
146 | int3 t = t0; | ||
147 | int n = tris.Count; | ||
148 | HullTriangle ta = new HullTriangle(v, t[1], t[2], tris); | ||
149 | ta.n = new int3(t0.n[0], n + 1, n + 2); | ||
150 | tris[t0.n[0]].setneib(t[1], t[2], n + 0); | ||
151 | HullTriangle tb = new HullTriangle(v, t[2], t[0], tris); | ||
152 | tb.n = new int3(t0.n[1], n + 2, n + 0); | ||
153 | tris[t0.n[1]].setneib(t[2], t[0], n + 1); | ||
154 | HullTriangle tc = new HullTriangle(v, t[0], t[1], tris); | ||
155 | tc.n = new int3(t0.n[2], n + 0, n + 1); | ||
156 | tris[t0.n[2]].setneib(t[0], t[1], n + 2); | ||
157 | checkit(ta, tris); | ||
158 | checkit(tb, tris); | ||
159 | checkit(tc, tris); | ||
160 | if (hasvert(tris[ta.n[0]], v)) | ||
161 | removeb2b(ta, tris[ta.n[0]], tris); | ||
162 | if (hasvert(tris[tb.n[0]], v)) | ||
163 | removeb2b(tb, tris[tb.n[0]], tris); | ||
164 | if (hasvert(tris[tc.n[0]], v)) | ||
165 | removeb2b(tc, tris[tc.n[0]], tris); | ||
166 | t0.Dispose(); | ||
167 | } | ||
168 | |||
169 | public static HullTriangle extrudable(float epsilon, List<HullTriangle> tris) | ||
170 | { | ||
171 | int i; | ||
172 | HullTriangle t = null; | ||
173 | for (i = 0; i < tris.Count; i++) | ||
174 | { | ||
175 | if (t == null || (tris.Count > i && (object)tris[i] != null && t.rise < tris[i].rise)) | ||
176 | { | ||
177 | t = tris[i]; | ||
178 | } | ||
179 | } | ||
180 | return (t.rise > epsilon) ? t : null; | ||
181 | } | ||
182 | |||
183 | public static Quaternion RotationArc(float3 v0, float3 v1) | ||
184 | { | ||
185 | Quaternion q = new Quaternion(); | ||
186 | v0 = float3.normalize(v0); // Comment these two lines out if you know its not needed. | ||
187 | v1 = float3.normalize(v1); // If vector is already unit length then why do it again? | ||
188 | float3 c = float3.cross(v0, v1); | ||
189 | float d = float3.dot(v0, v1); | ||
190 | if (d <= -1.0f) // 180 about x axis | ||
191 | { | ||
192 | return new Quaternion(1f, 0f, 0f, 0f); | ||
193 | } | ||
194 | float s = (float)Math.Sqrt((1 + d) * 2f); | ||
195 | q.x = c.x / s; | ||
196 | q.y = c.y / s; | ||
197 | q.z = c.z / s; | ||
198 | q.w = s / 2.0f; | ||
199 | return q; | ||
200 | } | ||
201 | |||
202 | public static float3 PlaneLineIntersection(Plane plane, float3 p0, float3 p1) | ||
203 | { | ||
204 | // returns the point where the line p0-p1 intersects the plane n&d | ||
205 | float3 dif = p1 - p0; | ||
206 | float dn = float3.dot(plane.normal, dif); | ||
207 | float t = -(plane.dist + float3.dot(plane.normal, p0)) / dn; | ||
208 | return p0 + (dif * t); | ||
209 | } | ||
210 | |||
211 | public static float3 LineProject(float3 p0, float3 p1, float3 a) | ||
212 | { | ||
213 | float3 w = new float3(); | ||
214 | w = p1 - p0; | ||
215 | float t = float3.dot(w, (a - p0)) / (w.x * w.x + w.y * w.y + w.z * w.z); | ||
216 | return p0 + w * t; | ||
217 | } | ||
218 | |||
219 | public static float3 PlaneProject(Plane plane, float3 point) | ||
220 | { | ||
221 | return point - plane.normal * (float3.dot(point, plane.normal) + plane.dist); | ||
222 | } | ||
223 | |||
224 | public static float LineProjectTime(float3 p0, float3 p1, float3 a) | ||
225 | { | ||
226 | float3 w = new float3(); | ||
227 | w = p1 - p0; | ||
228 | float t = float3.dot(w, (a - p0)) / (w.x * w.x + w.y * w.y + w.z * w.z); | ||
229 | return t; | ||
230 | } | ||
231 | |||
232 | public static float3 ThreePlaneIntersection(Plane p0, Plane p1, Plane p2) | ||
233 | { | ||
234 | float3x3 mp = float3x3.Transpose(new float3x3(p0.normal, p1.normal, p2.normal)); | ||
235 | float3x3 mi = float3x3.Inverse(mp); | ||
236 | float3 b = new float3(p0.dist, p1.dist, p2.dist); | ||
237 | return -b * mi; | ||
238 | } | ||
239 | |||
240 | public static bool PolyHit(List<float3> vert, float3 v0, float3 v1) | ||
241 | { | ||
242 | float3 impact = new float3(); | ||
243 | float3 normal = new float3(); | ||
244 | return PolyHit(vert, v0, v1, out impact, out normal); | ||
245 | } | ||
246 | |||
247 | public static bool PolyHit(List<float3> vert, float3 v0, float3 v1, out float3 impact) | ||
248 | { | ||
249 | float3 normal = new float3(); | ||
250 | return PolyHit(vert, v0, v1, out impact, out normal); | ||
251 | } | ||
252 | |||
253 | public static bool PolyHit(List<float3> vert, float3 v0, float3 v1, out float3 impact, out float3 normal) | ||
254 | { | ||
255 | float3 the_point = new float3(); | ||
256 | |||
257 | impact = null; | ||
258 | normal = null; | ||
259 | |||
260 | int i; | ||
261 | float3 nrml = new float3(0, 0, 0); | ||
262 | for (i = 0; i < vert.Count; i++) | ||
263 | { | ||
264 | int i1 = (i + 1) % vert.Count; | ||
265 | int i2 = (i + 2) % vert.Count; | ||
266 | nrml = nrml + float3.cross(vert[i1] - vert[i], vert[i2] - vert[i1]); | ||
267 | } | ||
268 | |||
269 | float m = float3.magnitude(nrml); | ||
270 | if (m == 0.0) | ||
271 | { | ||
272 | return false; | ||
273 | } | ||
274 | nrml = nrml * (1.0f / m); | ||
275 | float dist = -float3.dot(nrml, vert[0]); | ||
276 | float d0; | ||
277 | float d1; | ||
278 | if ((d0 = float3.dot(v0, nrml) + dist) < 0 || (d1 = float3.dot(v1, nrml) + dist) > 0) | ||
279 | { | ||
280 | return false; | ||
281 | } | ||
282 | |||
283 | // By using the cached plane distances d0 and d1 | ||
284 | // we can optimize the following: | ||
285 | // the_point = planelineintersection(nrml,dist,v0,v1); | ||
286 | float a = d0 / (d0 - d1); | ||
287 | the_point = v0 * (1 - a) + v1 * a; | ||
288 | |||
289 | |||
290 | bool inside = true; | ||
291 | for (int j = 0; inside && j < vert.Count; j++) | ||
292 | { | ||
293 | // let inside = 0 if outside | ||
294 | float3 pp1 = new float3(); | ||
295 | float3 pp2 = new float3(); | ||
296 | float3 side = new float3(); | ||
297 | pp1 = vert[j]; | ||
298 | pp2 = vert[(j + 1) % vert.Count]; | ||
299 | side = float3.cross((pp2 - pp1), (the_point - pp1)); | ||
300 | inside = (float3.dot(nrml, side) >= 0.0); | ||
301 | } | ||
302 | if (inside) | ||
303 | { | ||
304 | if (normal != null) | ||
305 | { | ||
306 | normal = nrml; | ||
307 | } | ||
308 | if (impact != null) | ||
309 | { | ||
310 | impact = the_point; | ||
311 | } | ||
312 | } | ||
313 | return inside; | ||
314 | } | ||
315 | |||
316 | public static bool BoxInside(float3 p, float3 bmin, float3 bmax) | ||
317 | { | ||
318 | return (p.x >= bmin.x && p.x <= bmax.x && p.y >= bmin.y && p.y <= bmax.y && p.z >= bmin.z && p.z <= bmax.z); | ||
319 | } | ||
320 | |||
321 | public static bool BoxIntersect(float3 v0, float3 v1, float3 bmin, float3 bmax, float3 impact) | ||
322 | { | ||
323 | if (BoxInside(v0, bmin, bmax)) | ||
324 | { | ||
325 | impact = v0; | ||
326 | return true; | ||
327 | } | ||
328 | if (v0.x <= bmin.x && v1.x >= bmin.x) | ||
329 | { | ||
330 | float a = (bmin.x - v0.x) / (v1.x - v0.x); | ||
331 | //v.x = bmin.x; | ||
332 | float vy = (1 - a) * v0.y + a * v1.y; | ||
333 | float vz = (1 - a) * v0.z + a * v1.z; | ||
334 | if (vy >= bmin.y && vy <= bmax.y && vz >= bmin.z && vz <= bmax.z) | ||
335 | { | ||
336 | impact.x = bmin.x; | ||
337 | impact.y = vy; | ||
338 | impact.z = vz; | ||
339 | return true; | ||
340 | } | ||
341 | } | ||
342 | else if (v0.x >= bmax.x && v1.x <= bmax.x) | ||
343 | { | ||
344 | float a = (bmax.x - v0.x) / (v1.x - v0.x); | ||
345 | //v.x = bmax.x; | ||
346 | float vy = (1 - a) * v0.y + a * v1.y; | ||
347 | float vz = (1 - a) * v0.z + a * v1.z; | ||
348 | if (vy >= bmin.y && vy <= bmax.y && vz >= bmin.z && vz <= bmax.z) | ||
349 | { | ||
350 | impact.x = bmax.x; | ||
351 | impact.y = vy; | ||
352 | impact.z = vz; | ||
353 | return true; | ||
354 | } | ||
355 | } | ||
356 | if (v0.y <= bmin.y && v1.y >= bmin.y) | ||
357 | { | ||
358 | float a = (bmin.y - v0.y) / (v1.y - v0.y); | ||
359 | float vx = (1 - a) * v0.x + a * v1.x; | ||
360 | //v.y = bmin.y; | ||
361 | float vz = (1 - a) * v0.z + a * v1.z; | ||
362 | if (vx >= bmin.x && vx <= bmax.x && vz >= bmin.z && vz <= bmax.z) | ||
363 | { | ||
364 | impact.x = vx; | ||
365 | impact.y = bmin.y; | ||
366 | impact.z = vz; | ||
367 | return true; | ||
368 | } | ||
369 | } | ||
370 | else if (v0.y >= bmax.y && v1.y <= bmax.y) | ||
371 | { | ||
372 | float a = (bmax.y - v0.y) / (v1.y - v0.y); | ||
373 | float vx = (1 - a) * v0.x + a * v1.x; | ||
374 | // vy = bmax.y; | ||
375 | float vz = (1 - a) * v0.z + a * v1.z; | ||
376 | if (vx >= bmin.x && vx <= bmax.x && vz >= bmin.z && vz <= bmax.z) | ||
377 | { | ||
378 | impact.x = vx; | ||
379 | impact.y = bmax.y; | ||
380 | impact.z = vz; | ||
381 | return true; | ||
382 | } | ||
383 | } | ||
384 | if (v0.z <= bmin.z && v1.z >= bmin.z) | ||
385 | { | ||
386 | float a = (bmin.z - v0.z) / (v1.z - v0.z); | ||
387 | float vx = (1 - a) * v0.x + a * v1.x; | ||
388 | float vy = (1 - a) * v0.y + a * v1.y; | ||
389 | // v.z = bmin.z; | ||
390 | if (vy >= bmin.y && vy <= bmax.y && vx >= bmin.x && vx <= bmax.x) | ||
391 | { | ||
392 | impact.x = vx; | ||
393 | impact.y = vy; | ||
394 | impact.z = bmin.z; | ||
395 | return true; | ||
396 | } | ||
397 | } | ||
398 | else if (v0.z >= bmax.z && v1.z <= bmax.z) | ||
399 | { | ||
400 | float a = (bmax.z - v0.z) / (v1.z - v0.z); | ||
401 | float vx = (1 - a) * v0.x + a * v1.x; | ||
402 | float vy = (1 - a) * v0.y + a * v1.y; | ||
403 | // v.z = bmax.z; | ||
404 | if (vy >= bmin.y && vy <= bmax.y && vx >= bmin.x && vx <= bmax.x) | ||
405 | { | ||
406 | impact.x = vx; | ||
407 | impact.y = vy; | ||
408 | impact.z = bmax.z; | ||
409 | return true; | ||
410 | } | ||
411 | } | ||
412 | return false; | ||
413 | } | ||
414 | |||
415 | public static float DistanceBetweenLines(float3 ustart, float3 udir, float3 vstart, float3 vdir, float3 upoint) | ||
416 | { | ||
417 | return DistanceBetweenLines(ustart, udir, vstart, vdir, upoint, null); | ||
418 | } | ||
419 | |||
420 | public static float DistanceBetweenLines(float3 ustart, float3 udir, float3 vstart, float3 vdir) | ||
421 | { | ||
422 | return DistanceBetweenLines(ustart, udir, vstart, vdir, null, null); | ||
423 | } | ||
424 | |||
425 | public static float DistanceBetweenLines(float3 ustart, float3 udir, float3 vstart, float3 vdir, float3 upoint, float3 vpoint) | ||
426 | { | ||
427 | float3 cp = float3.normalize(float3.cross(udir, vdir)); | ||
428 | |||
429 | float distu = -float3.dot(cp, ustart); | ||
430 | float distv = -float3.dot(cp, vstart); | ||
431 | float dist = (float)Math.Abs(distu - distv); | ||
432 | if (upoint != null) | ||
433 | { | ||
434 | Plane plane = new Plane(); | ||
435 | plane.normal = float3.normalize(float3.cross(vdir, cp)); | ||
436 | plane.dist = -float3.dot(plane.normal, vstart); | ||
437 | upoint = PlaneLineIntersection(plane, ustart, ustart + udir); | ||
438 | } | ||
439 | if (vpoint != null) | ||
440 | { | ||
441 | Plane plane = new Plane(); | ||
442 | plane.normal = float3.normalize(float3.cross(udir, cp)); | ||
443 | plane.dist = -float3.dot(plane.normal, ustart); | ||
444 | vpoint = PlaneLineIntersection(plane, vstart, vstart + vdir); | ||
445 | } | ||
446 | return dist; | ||
447 | } | ||
448 | |||
449 | public static float3 TriNormal(float3 v0, float3 v1, float3 v2) | ||
450 | { | ||
451 | // return the normal of the triangle | ||
452 | // inscribed by v0, v1, and v2 | ||
453 | float3 cp = float3.cross(v1 - v0, v2 - v1); | ||
454 | float m = float3.magnitude(cp); | ||
455 | if (m == 0) | ||
456 | return new float3(1, 0, 0); | ||
457 | return cp * (1.0f / m); | ||
458 | } | ||
459 | |||
460 | public static int PlaneTest(Plane p, float3 v, float planetestepsilon) | ||
461 | { | ||
462 | float a = float3.dot(v, p.normal) + p.dist; | ||
463 | int flag = (a > planetestepsilon) ? (2) : ((a < -planetestepsilon) ? (1) : (0)); | ||
464 | return flag; | ||
465 | } | ||
466 | |||
467 | public static int SplitTest(ref ConvexH convex, Plane plane, float planetestepsilon) | ||
468 | { | ||
469 | int flag = 0; | ||
470 | for (int i = 0; i < convex.vertices.Count; i++) | ||
471 | { | ||
472 | flag |= PlaneTest(plane, convex.vertices[i], planetestepsilon); | ||
473 | } | ||
474 | return flag; | ||
475 | } | ||
476 | |||
477 | public static Quaternion VirtualTrackBall(float3 cop, float3 cor, float3 dir1, float3 dir2) | ||
478 | { | ||
479 | // routine taken from game programming gems. | ||
480 | // Implement track ball functionality to spin stuf on the screen | ||
481 | // cop center of projection | ||
482 | // cor center of rotation | ||
483 | // dir1 old mouse direction | ||
484 | // dir2 new mouse direction | ||
485 | // pretend there is a sphere around cor. Then find the points | ||
486 | // where dir1 and dir2 intersect that sphere. Find the | ||
487 | // rotation that takes the first point to the second. | ||
488 | float m; | ||
489 | // compute plane | ||
490 | float3 nrml = cor - cop; | ||
491 | float fudgefactor = 1.0f / (float3.magnitude(nrml) * 0.25f); // since trackball proportional to distance from cop | ||
492 | nrml = float3.normalize(nrml); | ||
493 | float dist = -float3.dot(nrml, cor); | ||
494 | float3 u = PlaneLineIntersection(new Plane(nrml, dist), cop, cop + dir1); | ||
495 | u = u - cor; | ||
496 | u = u * fudgefactor; | ||
497 | m = float3.magnitude(u); | ||
498 | if (m > 1) | ||
499 | { | ||
500 | u /= m; | ||
501 | } | ||
502 | else | ||
503 | { | ||
504 | u = u - (nrml * (float)Math.Sqrt(1 - m * m)); | ||
505 | } | ||
506 | float3 v = PlaneLineIntersection(new Plane(nrml, dist), cop, cop + dir2); | ||
507 | v = v - cor; | ||
508 | v = v * fudgefactor; | ||
509 | m = float3.magnitude(v); | ||
510 | if (m > 1) | ||
511 | { | ||
512 | v /= m; | ||
513 | } | ||
514 | else | ||
515 | { | ||
516 | v = v - (nrml * (float)Math.Sqrt(1 - m * m)); | ||
517 | } | ||
518 | return RotationArc(u, v); | ||
519 | } | ||
520 | |||
521 | public static bool AssertIntact(ConvexH convex, float planetestepsilon) | ||
522 | { | ||
523 | int i; | ||
524 | int estart = 0; | ||
525 | for (i = 0; i < convex.edges.Count; i++) | ||
526 | { | ||
527 | if (convex.edges[estart].p != convex.edges[i].p) | ||
528 | { | ||
529 | estart = i; | ||
530 | } | ||
531 | int inext = i + 1; | ||
532 | if (inext >= convex.edges.Count || convex.edges[inext].p != convex.edges[i].p) | ||
533 | { | ||
534 | inext = estart; | ||
535 | } | ||
536 | Debug.Assert(convex.edges[inext].p == convex.edges[i].p); | ||
537 | int nb = convex.edges[i].ea; | ||
538 | Debug.Assert(nb != 255); | ||
539 | if (nb == 255 || nb == -1) | ||
540 | return false; | ||
541 | Debug.Assert(nb != -1); | ||
542 | Debug.Assert(i == convex.edges[nb].ea); | ||
543 | } | ||
544 | for (i = 0; i < convex.edges.Count; i++) | ||
545 | { | ||
546 | Debug.Assert((0) == PlaneTest(convex.facets[convex.edges[i].p], convex.vertices[convex.edges[i].v], planetestepsilon)); | ||
547 | if ((0) != PlaneTest(convex.facets[convex.edges[i].p], convex.vertices[convex.edges[i].v], planetestepsilon)) | ||
548 | return false; | ||
549 | if (convex.edges[estart].p != convex.edges[i].p) | ||
550 | { | ||
551 | estart = i; | ||
552 | } | ||
553 | int i1 = i + 1; | ||
554 | if (i1 >= convex.edges.Count || convex.edges[i1].p != convex.edges[i].p) | ||
555 | { | ||
556 | i1 = estart; | ||
557 | } | ||
558 | int i2 = i1 + 1; | ||
559 | if (i2 >= convex.edges.Count || convex.edges[i2].p != convex.edges[i].p) | ||
560 | { | ||
561 | i2 = estart; | ||
562 | } | ||
563 | if (i == i2) // i sliced tangent to an edge and created 2 meaningless edges | ||
564 | continue; | ||
565 | float3 localnormal = TriNormal(convex.vertices[convex.edges[i].v], convex.vertices[convex.edges[i1].v], convex.vertices[convex.edges[i2].v]); | ||
566 | Debug.Assert(float3.dot(localnormal, convex.facets[convex.edges[i].p].normal) > 0); | ||
567 | if (float3.dot(localnormal, convex.facets[convex.edges[i].p].normal) <= 0) | ||
568 | return false; | ||
569 | } | ||
570 | return true; | ||
571 | } | ||
572 | |||
573 | public static ConvexH test_btbq(float planetestepsilon) | ||
574 | { | ||
575 | // back to back quads | ||
576 | ConvexH convex = new ConvexH(4, 8, 2); | ||
577 | convex.vertices[0] = new float3(0, 0, 0); | ||
578 | convex.vertices[1] = new float3(1, 0, 0); | ||
579 | convex.vertices[2] = new float3(1, 1, 0); | ||
580 | convex.vertices[3] = new float3(0, 1, 0); | ||
581 | convex.facets[0] = new Plane(new float3(0, 0, 1), 0); | ||
582 | convex.facets[1] = new Plane(new float3(0, 0, -1), 0); | ||
583 | convex.edges[0] = new ConvexH.HalfEdge(7, 0, 0); | ||
584 | convex.edges[1] = new ConvexH.HalfEdge(6, 1, 0); | ||
585 | convex.edges[2] = new ConvexH.HalfEdge(5, 2, 0); | ||
586 | convex.edges[3] = new ConvexH.HalfEdge(4, 3, 0); | ||
587 | |||
588 | convex.edges[4] = new ConvexH.HalfEdge(3, 0, 1); | ||
589 | convex.edges[5] = new ConvexH.HalfEdge(2, 3, 1); | ||
590 | convex.edges[6] = new ConvexH.HalfEdge(1, 2, 1); | ||
591 | convex.edges[7] = new ConvexH.HalfEdge(0, 1, 1); | ||
592 | AssertIntact(convex, planetestepsilon); | ||
593 | return convex; | ||
594 | } | ||
595 | |||
596 | public static ConvexH test_cube() | ||
597 | { | ||
598 | ConvexH convex = new ConvexH(8, 24, 6); | ||
599 | convex.vertices[0] = new float3(0, 0, 0); | ||
600 | convex.vertices[1] = new float3(0, 0, 1); | ||
601 | convex.vertices[2] = new float3(0, 1, 0); | ||
602 | convex.vertices[3] = new float3(0, 1, 1); | ||
603 | convex.vertices[4] = new float3(1, 0, 0); | ||
604 | convex.vertices[5] = new float3(1, 0, 1); | ||
605 | convex.vertices[6] = new float3(1, 1, 0); | ||
606 | convex.vertices[7] = new float3(1, 1, 1); | ||
607 | |||
608 | convex.facets[0] = new Plane(new float3(-1, 0, 0), 0); | ||
609 | convex.facets[1] = new Plane(new float3(1, 0, 0), -1); | ||
610 | convex.facets[2] = new Plane(new float3(0, -1, 0), 0); | ||
611 | convex.facets[3] = new Plane(new float3(0, 1, 0), -1); | ||
612 | convex.facets[4] = new Plane(new float3(0, 0, -1), 0); | ||
613 | convex.facets[5] = new Plane(new float3(0, 0, 1), -1); | ||
614 | |||
615 | convex.edges[0] = new ConvexH.HalfEdge(11, 0, 0); | ||
616 | convex.edges[1] = new ConvexH.HalfEdge(23, 1, 0); | ||
617 | convex.edges[2] = new ConvexH.HalfEdge(15, 3, 0); | ||
618 | convex.edges[3] = new ConvexH.HalfEdge(16, 2, 0); | ||
619 | |||
620 | convex.edges[4] = new ConvexH.HalfEdge(13, 6, 1); | ||
621 | convex.edges[5] = new ConvexH.HalfEdge(21, 7, 1); | ||
622 | convex.edges[6] = new ConvexH.HalfEdge(9, 5, 1); | ||
623 | convex.edges[7] = new ConvexH.HalfEdge(18, 4, 1); | ||
624 | |||
625 | convex.edges[8] = new ConvexH.HalfEdge(19, 0, 2); | ||
626 | convex.edges[9] = new ConvexH.HalfEdge(6, 4, 2); | ||
627 | convex.edges[10] = new ConvexH.HalfEdge(20, 5, 2); | ||
628 | convex.edges[11] = new ConvexH.HalfEdge(0, 1, 2); | ||
629 | |||
630 | convex.edges[12] = new ConvexH.HalfEdge(22, 3, 3); | ||
631 | convex.edges[13] = new ConvexH.HalfEdge(4, 7, 3); | ||
632 | convex.edges[14] = new ConvexH.HalfEdge(17, 6, 3); | ||
633 | convex.edges[15] = new ConvexH.HalfEdge(2, 2, 3); | ||
634 | |||
635 | convex.edges[16] = new ConvexH.HalfEdge(3, 0, 4); | ||
636 | convex.edges[17] = new ConvexH.HalfEdge(14, 2, 4); | ||
637 | convex.edges[18] = new ConvexH.HalfEdge(7, 6, 4); | ||
638 | convex.edges[19] = new ConvexH.HalfEdge(8, 4, 4); | ||
639 | |||
640 | convex.edges[20] = new ConvexH.HalfEdge(10, 1, 5); | ||
641 | convex.edges[21] = new ConvexH.HalfEdge(5, 5, 5); | ||
642 | convex.edges[22] = new ConvexH.HalfEdge(12, 7, 5); | ||
643 | convex.edges[23] = new ConvexH.HalfEdge(1, 3, 5); | ||
644 | |||
645 | return convex; | ||
646 | } | ||
647 | |||
648 | public static ConvexH ConvexHMakeCube(float3 bmin, float3 bmax) | ||
649 | { | ||
650 | ConvexH convex = test_cube(); | ||
651 | convex.vertices[0] = new float3(bmin.x, bmin.y, bmin.z); | ||
652 | convex.vertices[1] = new float3(bmin.x, bmin.y, bmax.z); | ||
653 | convex.vertices[2] = new float3(bmin.x, bmax.y, bmin.z); | ||
654 | convex.vertices[3] = new float3(bmin.x, bmax.y, bmax.z); | ||
655 | convex.vertices[4] = new float3(bmax.x, bmin.y, bmin.z); | ||
656 | convex.vertices[5] = new float3(bmax.x, bmin.y, bmax.z); | ||
657 | convex.vertices[6] = new float3(bmax.x, bmax.y, bmin.z); | ||
658 | convex.vertices[7] = new float3(bmax.x, bmax.y, bmax.z); | ||
659 | |||
660 | convex.facets[0] = new Plane(new float3(-1, 0, 0), bmin.x); | ||
661 | convex.facets[1] = new Plane(new float3(1, 0, 0), -bmax.x); | ||
662 | convex.facets[2] = new Plane(new float3(0, -1, 0), bmin.y); | ||
663 | convex.facets[3] = new Plane(new float3(0, 1, 0), -bmax.y); | ||
664 | convex.facets[4] = new Plane(new float3(0, 0, -1), bmin.z); | ||
665 | convex.facets[5] = new Plane(new float3(0, 0, 1), -bmax.z); | ||
666 | return convex; | ||
667 | } | ||
668 | |||
669 | public static ConvexH ConvexHCrop(ref ConvexH convex, Plane slice, float planetestepsilon) | ||
670 | { | ||
671 | int i; | ||
672 | int vertcountunder = 0; | ||
673 | int vertcountover = 0; | ||
674 | List<int> vertscoplanar = new List<int>(); // existing vertex members of convex that are coplanar | ||
675 | List<int> edgesplit = new List<int>(); // existing edges that members of convex that cross the splitplane | ||
676 | |||
677 | Debug.Assert(convex.edges.Count < 480); | ||
678 | |||
679 | EdgeFlag[] edgeflag = new EdgeFlag[512]; | ||
680 | VertFlag[] vertflag = new VertFlag[256]; | ||
681 | PlaneFlag[] planeflag = new PlaneFlag[128]; | ||
682 | ConvexH.HalfEdge[] tmpunderedges = new ConvexH.HalfEdge[512]; | ||
683 | Plane[] tmpunderplanes = new Plane[128]; | ||
684 | Coplanar[] coplanaredges = new Coplanar[512]; | ||
685 | int coplanaredges_num = 0; | ||
686 | |||
687 | List<float3> createdverts = new List<float3>(); | ||
688 | |||
689 | // do the side-of-plane tests | ||
690 | for (i = 0; i < convex.vertices.Count; i++) | ||
691 | { | ||
692 | vertflag[i].planetest = (byte)PlaneTest(slice, convex.vertices[i], planetestepsilon); | ||
693 | if (vertflag[i].planetest == (0)) | ||
694 | { | ||
695 | // ? vertscoplanar.Add(i); | ||
696 | vertflag[i].undermap = (byte)vertcountunder++; | ||
697 | vertflag[i].overmap = (byte)vertcountover++; | ||
698 | } | ||
699 | else if (vertflag[i].planetest == (1)) | ||
700 | { | ||
701 | vertflag[i].undermap = (byte)vertcountunder++; | ||
702 | } | ||
703 | else | ||
704 | { | ||
705 | Debug.Assert(vertflag[i].planetest == (2)); | ||
706 | vertflag[i].overmap = (byte)vertcountover++; | ||
707 | vertflag[i].undermap = 255; // for debugging purposes | ||
708 | } | ||
709 | } | ||
710 | int vertcountunderold = vertcountunder; // for debugging only | ||
711 | |||
712 | int under_edge_count = 0; | ||
713 | int underplanescount = 0; | ||
714 | int e0 = 0; | ||
715 | |||
716 | for (int currentplane = 0; currentplane < convex.facets.Count; currentplane++) | ||
717 | { | ||
718 | int estart = e0; | ||
719 | int enextface = 0; | ||
720 | int planeside = 0; | ||
721 | int e1 = e0 + 1; | ||
722 | int vout = -1; | ||
723 | int vin = -1; | ||
724 | int coplanaredge = -1; | ||
725 | do | ||
726 | { | ||
727 | |||
728 | if (e1 >= convex.edges.Count || convex.edges[e1].p != currentplane) | ||
729 | { | ||
730 | enextface = e1; | ||
731 | e1 = estart; | ||
732 | } | ||
733 | ConvexH.HalfEdge edge0 = convex.edges[e0]; | ||
734 | ConvexH.HalfEdge edge1 = convex.edges[e1]; | ||
735 | ConvexH.HalfEdge edgea = convex.edges[edge0.ea]; | ||
736 | |||
737 | planeside |= vertflag[edge0.v].planetest; | ||
738 | //if((vertflag[edge0.v].planetest & vertflag[edge1.v].planetest) == COPLANAR) { | ||
739 | // assert(ecop==-1); | ||
740 | // ecop=e; | ||
741 | //} | ||
742 | |||
743 | if (vertflag[edge0.v].planetest == (2) && vertflag[edge1.v].planetest == (2)) | ||
744 | { | ||
745 | // both endpoints over plane | ||
746 | edgeflag[e0].undermap = -1; | ||
747 | } | ||
748 | else if ((vertflag[edge0.v].planetest | vertflag[edge1.v].planetest) == (1)) | ||
749 | { | ||
750 | // at least one endpoint under, the other coplanar or under | ||
751 | |||
752 | edgeflag[e0].undermap = (short)under_edge_count; | ||
753 | tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap; | ||
754 | tmpunderedges[under_edge_count].p = (byte)underplanescount; | ||
755 | if (edge0.ea < e0) | ||
756 | { | ||
757 | // connect the neighbors | ||
758 | Debug.Assert(edgeflag[edge0.ea].undermap != -1); | ||
759 | tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap; | ||
760 | tmpunderedges[edgeflag[edge0.ea].undermap].ea = (short)under_edge_count; | ||
761 | } | ||
762 | under_edge_count++; | ||
763 | } | ||
764 | else if ((vertflag[edge0.v].planetest | vertflag[edge1.v].planetest) == (0)) | ||
765 | { | ||
766 | // both endpoints coplanar | ||
767 | // must check a 3rd point to see if UNDER | ||
768 | int e2 = e1 + 1; | ||
769 | if (e2 >= convex.edges.Count || convex.edges[e2].p != currentplane) | ||
770 | { | ||
771 | e2 = estart; | ||
772 | } | ||
773 | Debug.Assert(convex.edges[e2].p == currentplane); | ||
774 | ConvexH.HalfEdge edge2 = convex.edges[e2]; | ||
775 | if (vertflag[edge2.v].planetest == (1)) | ||
776 | { | ||
777 | |||
778 | edgeflag[e0].undermap = (short)under_edge_count; | ||
779 | tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap; | ||
780 | tmpunderedges[under_edge_count].p = (byte)underplanescount; | ||
781 | tmpunderedges[under_edge_count].ea = -1; | ||
782 | // make sure this edge is added to the "coplanar" list | ||
783 | coplanaredge = under_edge_count; | ||
784 | vout = vertflag[edge0.v].undermap; | ||
785 | vin = vertflag[edge1.v].undermap; | ||
786 | under_edge_count++; | ||
787 | } | ||
788 | else | ||
789 | { | ||
790 | edgeflag[e0].undermap = -1; | ||
791 | } | ||
792 | } | ||
793 | else if (vertflag[edge0.v].planetest == (1) && vertflag[edge1.v].planetest == (2)) | ||
794 | { | ||
795 | // first is under 2nd is over | ||
796 | |||
797 | edgeflag[e0].undermap = (short)under_edge_count; | ||
798 | tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap; | ||
799 | tmpunderedges[under_edge_count].p = (byte)underplanescount; | ||
800 | if (edge0.ea < e0) | ||
801 | { | ||
802 | Debug.Assert(edgeflag[edge0.ea].undermap != -1); | ||
803 | // connect the neighbors | ||
804 | tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap; | ||
805 | tmpunderedges[edgeflag[edge0.ea].undermap].ea = (short)under_edge_count; | ||
806 | vout = tmpunderedges[edgeflag[edge0.ea].undermap].v; | ||
807 | } | ||
808 | else | ||
809 | { | ||
810 | Plane p0 = convex.facets[edge0.p]; | ||
811 | Plane pa = convex.facets[edgea.p]; | ||
812 | createdverts.Add(ThreePlaneIntersection(p0, pa, slice)); | ||
813 | //createdverts.Add(PlaneProject(slice,PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v]))); | ||
814 | //createdverts.Add(PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v])); | ||
815 | vout = vertcountunder++; | ||
816 | } | ||
817 | under_edge_count++; | ||
818 | /// hmmm something to think about: i might be able to output this edge regarless of | ||
819 | // wheter or not we know v-in yet. ok i;ll try this now: | ||
820 | tmpunderedges[under_edge_count].v = (byte)vout; | ||
821 | tmpunderedges[under_edge_count].p = (byte)underplanescount; | ||
822 | tmpunderedges[under_edge_count].ea = -1; | ||
823 | coplanaredge = under_edge_count; | ||
824 | under_edge_count++; | ||
825 | |||
826 | if (vin != -1) | ||
827 | { | ||
828 | // we previously processed an edge where we came under | ||
829 | // now we know about vout as well | ||
830 | |||
831 | // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!! | ||
832 | } | ||
833 | |||
834 | } | ||
835 | else if (vertflag[edge0.v].planetest == (0) && vertflag[edge1.v].planetest == (2)) | ||
836 | { | ||
837 | // first is coplanar 2nd is over | ||
838 | |||
839 | edgeflag[e0].undermap = -1; | ||
840 | vout = vertflag[edge0.v].undermap; | ||
841 | // I hate this but i have to make sure part of this face is UNDER before ouputting this vert | ||
842 | int k = estart; | ||
843 | Debug.Assert(edge0.p == currentplane); | ||
844 | while (!((planeside & 1) != 0) && k < convex.edges.Count && convex.edges[k].p == edge0.p) | ||
845 | { | ||
846 | planeside |= vertflag[convex.edges[k].v].planetest; | ||
847 | k++; | ||
848 | } | ||
849 | if ((planeside & 1) != 0) | ||
850 | { | ||
851 | tmpunderedges[under_edge_count].v = (byte)vout; | ||
852 | tmpunderedges[under_edge_count].p = (byte)underplanescount; | ||
853 | tmpunderedges[under_edge_count].ea = -1; | ||
854 | coplanaredge = under_edge_count; // hmmm should make a note of the edge # for later on | ||
855 | under_edge_count++; | ||
856 | |||
857 | } | ||
858 | } | ||
859 | else if (vertflag[edge0.v].planetest == (2) && vertflag[edge1.v].planetest == (1)) | ||
860 | { | ||
861 | // first is over next is under | ||
862 | // new vertex!!! | ||
863 | Debug.Assert(vin == -1); | ||
864 | if (e0 < edge0.ea) | ||
865 | { | ||
866 | Plane p0 = convex.facets[edge0.p]; | ||
867 | Plane pa = convex.facets[edgea.p]; | ||
868 | createdverts.Add(ThreePlaneIntersection(p0, pa, slice)); | ||
869 | //createdverts.Add(PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v])); | ||
870 | //createdverts.Add(PlaneProject(slice,PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v]))); | ||
871 | vin = vertcountunder++; | ||
872 | } | ||
873 | else | ||
874 | { | ||
875 | // find the new vertex that was created by edge[edge0.ea] | ||
876 | int nea = edgeflag[edge0.ea].undermap; | ||
877 | Debug.Assert(tmpunderedges[nea].p == tmpunderedges[nea + 1].p); | ||
878 | vin = tmpunderedges[nea + 1].v; | ||
879 | Debug.Assert(vin < vertcountunder); | ||
880 | Debug.Assert(vin >= vertcountunderold); // for debugging only | ||
881 | } | ||
882 | if (vout != -1) | ||
883 | { | ||
884 | // we previously processed an edge where we went over | ||
885 | // now we know vin too | ||
886 | // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!! | ||
887 | } | ||
888 | // output edge | ||
889 | tmpunderedges[under_edge_count].v = (byte)vin; | ||
890 | tmpunderedges[under_edge_count].p = (byte)underplanescount; | ||
891 | edgeflag[e0].undermap = (short)under_edge_count; | ||
892 | if (e0 > edge0.ea) | ||
893 | { | ||
894 | Debug.Assert(edgeflag[edge0.ea].undermap != -1); | ||
895 | // connect the neighbors | ||
896 | tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap; | ||
897 | tmpunderedges[edgeflag[edge0.ea].undermap].ea = (short)under_edge_count; | ||
898 | } | ||
899 | Debug.Assert(edgeflag[e0].undermap == under_edge_count); | ||
900 | under_edge_count++; | ||
901 | } | ||
902 | else if (vertflag[edge0.v].planetest == (2) && vertflag[edge1.v].planetest == (0)) | ||
903 | { | ||
904 | // first is over next is coplanar | ||
905 | |||
906 | edgeflag[e0].undermap = -1; | ||
907 | vin = vertflag[edge1.v].undermap; | ||
908 | Debug.Assert(vin != -1); | ||
909 | if (vout != -1) | ||
910 | { | ||
911 | // we previously processed an edge where we came under | ||
912 | // now we know both endpoints | ||
913 | // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!! | ||
914 | } | ||
915 | |||
916 | } | ||
917 | else | ||
918 | { | ||
919 | Debug.Assert(false); | ||
920 | } | ||
921 | |||
922 | |||
923 | e0 = e1; | ||
924 | e1++; // do the modulo at the beginning of the loop | ||
925 | |||
926 | } while (e0 != estart); | ||
927 | e0 = enextface; | ||
928 | if ((planeside & 1) != 0) | ||
929 | { | ||
930 | planeflag[currentplane].undermap = (byte)underplanescount; | ||
931 | tmpunderplanes[underplanescount] = convex.facets[currentplane]; | ||
932 | underplanescount++; | ||
933 | } | ||
934 | else | ||
935 | { | ||
936 | planeflag[currentplane].undermap = 0; | ||
937 | } | ||
938 | if (vout >= 0 && (planeside & 1) != 0) | ||
939 | { | ||
940 | Debug.Assert(vin >= 0); | ||
941 | Debug.Assert(coplanaredge >= 0); | ||
942 | Debug.Assert(coplanaredge != 511); | ||
943 | coplanaredges[coplanaredges_num].ea = (ushort)coplanaredge; | ||
944 | coplanaredges[coplanaredges_num].v0 = (byte)vin; | ||
945 | coplanaredges[coplanaredges_num].v1 = (byte)vout; | ||
946 | coplanaredges_num++; | ||
947 | } | ||
948 | } | ||
949 | |||
950 | // add the new plane to the mix: | ||
951 | if (coplanaredges_num > 0) | ||
952 | { | ||
953 | tmpunderplanes[underplanescount++] = slice; | ||
954 | } | ||
955 | for (i = 0; i < coplanaredges_num - 1; i++) | ||
956 | { | ||
957 | if (coplanaredges[i].v1 != coplanaredges[i + 1].v0) | ||
958 | { | ||
959 | int j = 0; | ||
960 | for (j = i + 2; j < coplanaredges_num; j++) | ||
961 | { | ||
962 | if (coplanaredges[i].v1 == coplanaredges[j].v0) | ||
963 | { | ||
964 | Coplanar tmp = coplanaredges[i + 1]; | ||
965 | coplanaredges[i + 1] = coplanaredges[j]; | ||
966 | coplanaredges[j] = tmp; | ||
967 | break; | ||
968 | } | ||
969 | } | ||
970 | if (j >= coplanaredges_num) | ||
971 | { | ||
972 | Debug.Assert(j < coplanaredges_num); | ||
973 | return null; | ||
974 | } | ||
975 | } | ||
976 | } | ||
977 | |||
978 | ConvexH punder = new ConvexH(vertcountunder, under_edge_count + coplanaredges_num, underplanescount); | ||
979 | ConvexH under = punder; | ||
980 | |||
981 | { | ||
982 | int k = 0; | ||
983 | for (i = 0; i < convex.vertices.Count; i++) | ||
984 | { | ||
985 | if (vertflag[i].planetest != (2)) | ||
986 | { | ||
987 | under.vertices[k++] = convex.vertices[i]; | ||
988 | } | ||
989 | } | ||
990 | i = 0; | ||
991 | while (k < vertcountunder) | ||
992 | { | ||
993 | under.vertices[k++] = createdverts[i++]; | ||
994 | } | ||
995 | Debug.Assert(i == createdverts.Count); | ||
996 | } | ||
997 | |||
998 | for (i = 0; i < coplanaredges_num; i++) | ||
999 | { | ||
1000 | ConvexH.HalfEdge edge = under.edges[under_edge_count + i]; | ||
1001 | edge.p = (byte)(underplanescount - 1); | ||
1002 | edge.ea = (short)coplanaredges[i].ea; | ||
1003 | edge.v = (byte)coplanaredges[i].v0; | ||
1004 | under.edges[under_edge_count + i] = edge; | ||
1005 | |||
1006 | tmpunderedges[coplanaredges[i].ea].ea = (short)(under_edge_count + i); | ||
1007 | } | ||
1008 | |||
1009 | under.edges = new List<ConvexH.HalfEdge>(tmpunderedges); | ||
1010 | under.facets = new List<Plane>(tmpunderplanes); | ||
1011 | return punder; | ||
1012 | } | ||
1013 | |||
1014 | public static ConvexH ConvexHDup(ConvexH src) | ||
1015 | { | ||
1016 | ConvexH dst = new ConvexH(src.vertices.Count, src.edges.Count, src.facets.Count); | ||
1017 | dst.vertices = new List<float3>(src.vertices.Count); | ||
1018 | foreach (float3 f in src.vertices) | ||
1019 | dst.vertices.Add(new float3(f)); | ||
1020 | dst.edges = new List<ConvexH.HalfEdge>(src.edges.Count); | ||
1021 | foreach (ConvexH.HalfEdge e in src.edges) | ||
1022 | dst.edges.Add(new ConvexH.HalfEdge(e)); | ||
1023 | dst.facets = new List<Plane>(src.facets.Count); | ||
1024 | foreach (Plane p in src.facets) | ||
1025 | dst.facets.Add(new Plane(p)); | ||
1026 | return dst; | ||
1027 | } | ||
1028 | |||
1029 | public static int candidateplane(List<Plane> planes, int planes_count, ConvexH convex, float epsilon) | ||
1030 | { | ||
1031 | int p = 0; | ||
1032 | float md = 0; | ||
1033 | int i; | ||
1034 | for (i = 0; i < planes_count; i++) | ||
1035 | { | ||
1036 | float d = 0; | ||
1037 | for (int j = 0; j < convex.vertices.Count; j++) | ||
1038 | { | ||
1039 | d = Math.Max(d, float3.dot(convex.vertices[j], planes[i].normal) + planes[i].dist); | ||
1040 | } | ||
1041 | if (i == 0 || d > md) | ||
1042 | { | ||
1043 | p = i; | ||
1044 | md = d; | ||
1045 | } | ||
1046 | } | ||
1047 | return (md > epsilon) ? p : -1; | ||
1048 | } | ||
1049 | |||
1050 | public static float3 orth(float3 v) | ||
1051 | { | ||
1052 | float3 a = float3.cross(v, new float3(0f, 0f, 1f)); | ||
1053 | float3 b = float3.cross(v, new float3(0f, 1f, 0f)); | ||
1054 | return float3.normalize((float3.magnitude(a) > float3.magnitude(b)) ? a : b); | ||
1055 | } | ||
1056 | |||
1057 | public static int maxdir(List<float3> p, int count, float3 dir) | ||
1058 | { | ||
1059 | Debug.Assert(count != 0); | ||
1060 | int m = 0; | ||
1061 | float currDotm = float3.dot(p[0], dir); | ||
1062 | for (int i = 1; i < count; i++) | ||
1063 | { | ||
1064 | float currDoti = float3.dot(p[i], dir); | ||
1065 | if (currDoti > currDotm) | ||
1066 | { | ||
1067 | currDotm = currDoti; | ||
1068 | m = i; | ||
1069 | } | ||
1070 | } | ||
1071 | return m; | ||
1072 | } | ||
1073 | |||
1074 | public static int maxdirfiltered(List<float3> p, int count, float3 dir, byte[] allow) | ||
1075 | { | ||
1076 | //Debug.Assert(count != 0); | ||
1077 | int m = 0; | ||
1078 | float currDotm = float3.dot(p[0], dir); | ||
1079 | float currDoti; | ||
1080 | |||
1081 | while (allow[m] == 0) | ||
1082 | m++; | ||
1083 | |||
1084 | for (int i = 1; i < count; i++) | ||
1085 | { | ||
1086 | if (allow[i] != 0) | ||
1087 | { | ||
1088 | currDoti = float3.dot(p[i], dir); | ||
1089 | if (currDoti > currDotm) | ||
1090 | { | ||
1091 | currDotm = currDoti; | ||
1092 | m = i; | ||
1093 | } | ||
1094 | } | ||
1095 | } | ||
1096 | //Debug.Assert(m != -1); | ||
1097 | return m; | ||
1098 | } | ||
1099 | |||
1100 | public static int maxdirsterid(List<float3> p, int count, float3 dir, byte[] allow) | ||
1101 | { | ||
1102 | int m = -1; | ||
1103 | while (m == -1) | ||
1104 | { | ||
1105 | m = maxdirfiltered(p, count, dir, allow); | ||
1106 | if (allow[m] == 3) | ||
1107 | return m; | ||
1108 | float3 u = orth(dir); | ||
1109 | float3 v = float3.cross(u, dir); | ||
1110 | int ma = -1; | ||
1111 | for (float x = 0.0f; x <= 360.0f; x += 45.0f) | ||
1112 | { | ||
1113 | int mb; | ||
1114 | { | ||
1115 | float s = (float)Math.Sin((3.14159264f / 180.0f) * (x)); | ||
1116 | float c = (float)Math.Cos((3.14159264f / 180.0f) * (x)); | ||
1117 | mb = maxdirfiltered(p, count, dir + (u * s + v * c) * 0.025f, allow); | ||
1118 | } | ||
1119 | if (ma == m && mb == m) | ||
1120 | { | ||
1121 | allow[m] = 3; | ||
1122 | return m; | ||
1123 | } | ||
1124 | if (ma != -1 && ma != mb) // Yuck - this is really ugly | ||
1125 | { | ||
1126 | int mc = ma; | ||
1127 | for (float xx = x - 40.0f; xx <= x; xx += 5.0f) | ||
1128 | { | ||
1129 | float s = (float)Math.Sin((3.14159264f / 180.0f) * (xx)); | ||
1130 | float c = (float)Math.Cos((3.14159264f / 180.0f) * (xx)); | ||
1131 | int md = maxdirfiltered(p, count, dir + (u * s + v * c) * 0.025f, allow); | ||
1132 | if (mc == m && md == m) | ||
1133 | { | ||
1134 | allow[m] = 3; | ||
1135 | return m; | ||
1136 | } | ||
1137 | mc = md; | ||
1138 | } | ||
1139 | } | ||
1140 | ma = mb; | ||
1141 | } | ||
1142 | allow[m] = 0; | ||
1143 | m = -1; | ||
1144 | } | ||
1145 | |||
1146 | Debug.Assert(false); | ||
1147 | return m; | ||
1148 | } | ||
1149 | |||
1150 | public static int4 FindSimplex(List<float3> verts, byte[] allow) | ||
1151 | { | ||
1152 | float3[] basis = new float3[3]; | ||
1153 | basis[0] = new float3(0.01f, 0.02f, 1.0f); | ||
1154 | int p0 = maxdirsterid(verts, verts.Count, basis[0], allow); | ||
1155 | int p1 = maxdirsterid(verts, verts.Count, -basis[0], allow); | ||
1156 | basis[0] = verts[p0] - verts[p1]; | ||
1157 | if (p0 == p1 || basis[0] == new float3(0, 0, 0)) | ||
1158 | return new int4(-1, -1, -1, -1); | ||
1159 | basis[1] = float3.cross(new float3(1, 0.02f, 0), basis[0]); | ||
1160 | basis[2] = float3.cross(new float3(-0.02f, 1, 0), basis[0]); | ||
1161 | basis[1] = float3.normalize((float3.magnitude(basis[1]) > float3.magnitude(basis[2])) ? basis[1] : basis[2]); | ||
1162 | int p2 = maxdirsterid(verts, verts.Count, basis[1], allow); | ||
1163 | if (p2 == p0 || p2 == p1) | ||
1164 | { | ||
1165 | p2 = maxdirsterid(verts, verts.Count, -basis[1], allow); | ||
1166 | } | ||
1167 | if (p2 == p0 || p2 == p1) | ||
1168 | return new int4(-1, -1, -1, -1); | ||
1169 | basis[1] = verts[p2] - verts[p0]; | ||
1170 | basis[2] = float3.normalize(float3.cross(basis[1], basis[0])); | ||
1171 | int p3 = maxdirsterid(verts, verts.Count, basis[2], allow); | ||
1172 | if (p3 == p0 || p3 == p1 || p3 == p2) | ||
1173 | p3 = maxdirsterid(verts, verts.Count, -basis[2], allow); | ||
1174 | if (p3 == p0 || p3 == p1 || p3 == p2) | ||
1175 | return new int4(-1, -1, -1, -1); | ||
1176 | Debug.Assert(!(p0 == p1 || p0 == p2 || p0 == p3 || p1 == p2 || p1 == p3 || p2 == p3)); | ||
1177 | if (float3.dot(verts[p3] - verts[p0], float3.cross(verts[p1] - verts[p0], verts[p2] - verts[p0])) < 0) | ||
1178 | { | ||
1179 | Swap(ref p2, ref p3); | ||
1180 | } | ||
1181 | return new int4(p0, p1, p2, p3); | ||
1182 | } | ||
1183 | |||
1184 | public static float GetDist(float px, float py, float pz, float3 p2) | ||
1185 | { | ||
1186 | float dx = px - p2.x; | ||
1187 | float dy = py - p2.y; | ||
1188 | float dz = pz - p2.z; | ||
1189 | |||
1190 | return dx * dx + dy * dy + dz * dz; | ||
1191 | } | ||
1192 | |||
1193 | public static void ReleaseHull(PHullResult result) | ||
1194 | { | ||
1195 | if (result.Indices != null) | ||
1196 | result.Indices = null; | ||
1197 | if (result.Vertices != null) | ||
1198 | result.Vertices = null; | ||
1199 | } | ||
1200 | |||
1201 | public static int calchullgen(List<float3> verts, int vlimit, List<HullTriangle> tris) | ||
1202 | { | ||
1203 | if (verts.Count < 4) | ||
1204 | return 0; | ||
1205 | if (vlimit == 0) | ||
1206 | vlimit = 1000000000; | ||
1207 | int j; | ||
1208 | float3 bmin = new float3(verts[0]); | ||
1209 | float3 bmax = new float3(verts[0]); | ||
1210 | List<int> isextreme = new List<int>(verts.Count); | ||
1211 | byte[] allow = new byte[verts.Count]; | ||
1212 | for (j = 0; j < verts.Count; j++) | ||
1213 | { | ||
1214 | allow[j] = 1; | ||
1215 | isextreme.Add(0); | ||
1216 | bmin = float3.VectorMin(bmin, verts[j]); | ||
1217 | bmax = float3.VectorMax(bmax, verts[j]); | ||
1218 | } | ||
1219 | float epsilon = float3.magnitude(bmax - bmin) * 0.001f; | ||
1220 | |||
1221 | int4 p = FindSimplex(verts, allow); | ||
1222 | if (p.x == -1) // simplex failed | ||
1223 | return 0; | ||
1224 | |||
1225 | float3 center = (verts[p[0]] + verts[p[1]] + verts[p[2]] + verts[p[3]]) / 4.0f; // a valid interior point | ||
1226 | HullTriangle t0 = new HullTriangle(p[2], p[3], p[1], tris); | ||
1227 | t0.n = new int3(2, 3, 1); | ||
1228 | HullTriangle t1 = new HullTriangle(p[3], p[2], p[0], tris); | ||
1229 | t1.n = new int3(3, 2, 0); | ||
1230 | HullTriangle t2 = new HullTriangle(p[0], p[1], p[3], tris); | ||
1231 | t2.n = new int3(0, 1, 3); | ||
1232 | HullTriangle t3 = new HullTriangle(p[1], p[0], p[2], tris); | ||
1233 | t3.n = new int3(1, 0, 2); | ||
1234 | isextreme[p[0]] = isextreme[p[1]] = isextreme[p[2]] = isextreme[p[3]] = 1; | ||
1235 | checkit(t0, tris); | ||
1236 | checkit(t1, tris); | ||
1237 | checkit(t2, tris); | ||
1238 | checkit(t3, tris); | ||
1239 | |||
1240 | for (j = 0; j < tris.Count; j++) | ||
1241 | { | ||
1242 | HullTriangle t = tris[j]; | ||
1243 | Debug.Assert((object)t != null); | ||
1244 | Debug.Assert(t.vmax < 0); | ||
1245 | float3 n = TriNormal(verts[(t)[0]], verts[(t)[1]], verts[(t)[2]]); | ||
1246 | t.vmax = maxdirsterid(verts, verts.Count, n, allow); | ||
1247 | t.rise = float3.dot(n, verts[t.vmax] - verts[(t)[0]]); | ||
1248 | } | ||
1249 | HullTriangle te; | ||
1250 | vlimit -= 4; | ||
1251 | while (vlimit > 0 && (te = extrudable(epsilon, tris)) != null) | ||
1252 | { | ||
1253 | int3 ti = te; | ||
1254 | int v = te.vmax; | ||
1255 | Debug.Assert(isextreme[v] == 0); // wtf we've already done this vertex | ||
1256 | isextreme[v] = 1; | ||
1257 | //if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already | ||
1258 | j = tris.Count; | ||
1259 | while (j-- != 0) | ||
1260 | { | ||
1261 | if (tris.Count <= j || (object)tris[j] == null) | ||
1262 | continue; | ||
1263 | int3 t = tris[j]; | ||
1264 | if (above(verts, t, verts[v], 0.01f * epsilon)) | ||
1265 | { | ||
1266 | extrude(tris[j], v, tris); | ||
1267 | } | ||
1268 | } | ||
1269 | // now check for those degenerate cases where we have a flipped triangle or a really skinny triangle | ||
1270 | j = tris.Count; | ||
1271 | while (j-- != 0) | ||
1272 | { | ||
1273 | if (tris.Count <= j || (object)tris[j] == null) | ||
1274 | continue; | ||
1275 | if (!hasvert(tris[j], v)) | ||
1276 | break; | ||
1277 | int3 nt = tris[j]; | ||
1278 | if (above(verts, nt, center, 0.01f * epsilon) || float3.magnitude(float3.cross(verts[nt[1]] - verts[nt[0]], verts[nt[2]] - verts[nt[1]])) < epsilon * epsilon * 0.1f) | ||
1279 | { | ||
1280 | HullTriangle nb = tris[tris[j].n[0]]; | ||
1281 | Debug.Assert(nb != null); | ||
1282 | Debug.Assert(!hasvert(nb, v)); | ||
1283 | Debug.Assert(nb.id < j); | ||
1284 | extrude(nb, v, tris); | ||
1285 | j = tris.Count; | ||
1286 | } | ||
1287 | } | ||
1288 | j = tris.Count; | ||
1289 | while (j-- != 0) | ||
1290 | { | ||
1291 | HullTriangle t = tris[j]; | ||
1292 | if (t == null) | ||
1293 | continue; | ||
1294 | if (t.vmax >= 0) | ||
1295 | break; | ||
1296 | float3 n = TriNormal(verts[(t)[0]], verts[(t)[1]], verts[(t)[2]]); | ||
1297 | t.vmax = maxdirsterid(verts, verts.Count, n, allow); | ||
1298 | if (isextreme[t.vmax] != 0) | ||
1299 | { | ||
1300 | t.vmax = -1; // already done that vertex - algorithm needs to be able to terminate. | ||
1301 | } | ||
1302 | else | ||
1303 | { | ||
1304 | t.rise = float3.dot(n, verts[t.vmax] - verts[(t)[0]]); | ||
1305 | } | ||
1306 | } | ||
1307 | vlimit--; | ||
1308 | } | ||
1309 | return 1; | ||
1310 | } | ||
1311 | |||
1312 | public static bool calchull(List<float3> verts, out List<int> tris_out, int vlimit, List<HullTriangle> tris) | ||
1313 | { | ||
1314 | tris_out = null; | ||
1315 | |||
1316 | int rc = calchullgen(verts, vlimit, tris); | ||
1317 | if (rc == 0) | ||
1318 | return false; | ||
1319 | List<int> ts = new List<int>(); | ||
1320 | for (int i = 0; i < tris.Count; i++) | ||
1321 | { | ||
1322 | if ((object)tris[i] != null) | ||
1323 | { | ||
1324 | for (int j = 0; j < 3; j++) | ||
1325 | ts.Add((tris[i])[j]); | ||
1326 | tris[i] = null; | ||
1327 | } | ||
1328 | } | ||
1329 | |||
1330 | tris_out = ts; | ||
1331 | tris.Clear(); | ||
1332 | return true; | ||
1333 | } | ||
1334 | |||
1335 | public static int calchullpbev(List<float3> verts, int vlimit, out List<Plane> planes, float bevangle, List<HullTriangle> tris) | ||
1336 | { | ||
1337 | int i; | ||
1338 | int j; | ||
1339 | planes = new List<Plane>(); | ||
1340 | int rc = calchullgen(verts, vlimit, tris); | ||
1341 | if (rc == 0) | ||
1342 | return 0; | ||
1343 | for (i = 0; i < tris.Count; i++) | ||
1344 | { | ||
1345 | if (tris[i] != null) | ||
1346 | { | ||
1347 | Plane p = new Plane(); | ||
1348 | HullTriangle t = tris[i]; | ||
1349 | p.normal = TriNormal(verts[(t)[0]], verts[(t)[1]], verts[(t)[2]]); | ||
1350 | p.dist = -float3.dot(p.normal, verts[(t)[0]]); | ||
1351 | planes.Add(p); | ||
1352 | for (j = 0; j < 3; j++) | ||
1353 | { | ||
1354 | if (t.n[j] < t.id) | ||
1355 | continue; | ||
1356 | HullTriangle s = tris[t.n[j]]; | ||
1357 | float3 snormal = TriNormal(verts[(s)[0]], verts[(s)[1]], verts[(s)[2]]); | ||
1358 | if (float3.dot(snormal, p.normal) >= Math.Cos(bevangle * (3.14159264f / 180.0f))) | ||
1359 | continue; | ||
1360 | float3 n = float3.normalize(snormal + p.normal); | ||
1361 | planes.Add(new Plane(n, -float3.dot(n, verts[maxdir(verts, verts.Count, n)]))); | ||
1362 | } | ||
1363 | } | ||
1364 | } | ||
1365 | |||
1366 | tris.Clear(); | ||
1367 | return 1; | ||
1368 | } | ||
1369 | |||
1370 | public static int overhull(List<Plane> planes, List<float3> verts, int maxplanes, out List<float3> verts_out, out List<int> faces_out, float inflate) | ||
1371 | { | ||
1372 | verts_out = null; | ||
1373 | faces_out = null; | ||
1374 | |||
1375 | int i; | ||
1376 | int j; | ||
1377 | if (verts.Count < 4) | ||
1378 | return 0; | ||
1379 | maxplanes = Math.Min(maxplanes, planes.Count); | ||
1380 | float3 bmin = new float3(verts[0]); | ||
1381 | float3 bmax = new float3(verts[0]); | ||
1382 | for (i = 0; i < verts.Count; i++) | ||
1383 | { | ||
1384 | bmin = float3.VectorMin(bmin, verts[i]); | ||
1385 | bmax = float3.VectorMax(bmax, verts[i]); | ||
1386 | } | ||
1387 | // float diameter = magnitude(bmax-bmin); | ||
1388 | // inflate *=diameter; // RELATIVE INFLATION | ||
1389 | bmin -= new float3(inflate, inflate, inflate); | ||
1390 | bmax += new float3(inflate, inflate, inflate); | ||
1391 | for (i = 0; i < planes.Count; i++) | ||
1392 | { | ||
1393 | planes[i].dist -= inflate; | ||
1394 | } | ||
1395 | float3 emin = new float3(bmin); | ||
1396 | float3 emax = new float3(bmax); | ||
1397 | float epsilon = float3.magnitude(emax - emin) * 0.025f; | ||
1398 | float planetestepsilon = float3.magnitude(emax - emin) * (0.001f); | ||
1399 | // todo: add bounding cube planes to force bevel. or try instead not adding the diameter expansion ??? must think. | ||
1400 | // ConvexH *convex = ConvexHMakeCube(bmin - float3(diameter,diameter,diameter),bmax+float3(diameter,diameter,diameter)); | ||
1401 | ConvexH c = ConvexHMakeCube(new float3(bmin), new float3(bmax)); | ||
1402 | int k; | ||
1403 | while (maxplanes-- != 0 && (k = candidateplane(planes, planes.Count, c, epsilon)) >= 0) | ||
1404 | { | ||
1405 | ConvexH tmp = c; | ||
1406 | c = ConvexHCrop(ref tmp, planes[k], planetestepsilon); | ||
1407 | if (c == null) // might want to debug this case better!!! | ||
1408 | { | ||
1409 | c = tmp; | ||
1410 | break; | ||
1411 | } | ||
1412 | if (AssertIntact(c, planetestepsilon) == false) // might want to debug this case better too!!! | ||
1413 | { | ||
1414 | c = tmp; | ||
1415 | break; | ||
1416 | } | ||
1417 | tmp.edges = null; | ||
1418 | tmp.facets = null; | ||
1419 | tmp.vertices = null; | ||
1420 | } | ||
1421 | |||
1422 | Debug.Assert(AssertIntact(c, planetestepsilon)); | ||
1423 | //return c; | ||
1424 | //C++ TO C# CONVERTER TODO TASK: The memory management function 'malloc' has no equivalent in C#: | ||
1425 | faces_out = new List<int>(); //(int)malloc(sizeof(int) * (1 + c.facets.Count + c.edges.Count)); // new int[1+c->facets.count+c->edges.count]; | ||
1426 | int faces_count_out = 0; | ||
1427 | i = 0; | ||
1428 | faces_out[faces_count_out++] = -1; | ||
1429 | k = 0; | ||
1430 | while (i < c.edges.Count) | ||
1431 | { | ||
1432 | j = 1; | ||
1433 | while (j + i < c.edges.Count && c.edges[i].p == c.edges[i + j].p) | ||
1434 | { | ||
1435 | j++; | ||
1436 | } | ||
1437 | faces_out[faces_count_out++] = j; | ||
1438 | while (j-- != 0) | ||
1439 | { | ||
1440 | faces_out[faces_count_out++] = c.edges[i].v; | ||
1441 | i++; | ||
1442 | } | ||
1443 | k++; | ||
1444 | } | ||
1445 | faces_out[0] = k; // number of faces. | ||
1446 | Debug.Assert(k == c.facets.Count); | ||
1447 | Debug.Assert(faces_count_out == 1 + c.facets.Count + c.edges.Count); | ||
1448 | verts_out = c.vertices; // new float3[c->vertices.count]; | ||
1449 | int verts_count_out = c.vertices.Count; | ||
1450 | for (i = 0; i < c.vertices.Count; i++) | ||
1451 | { | ||
1452 | verts_out[i] = new float3(c.vertices[i]); | ||
1453 | } | ||
1454 | |||
1455 | c.edges = null; | ||
1456 | c.facets = null; | ||
1457 | c.vertices = null; | ||
1458 | return 1; | ||
1459 | } | ||
1460 | |||
1461 | public static int overhullv(List<float3> verts, int maxplanes, out List<float3> verts_out, out List<int> faces_out, float inflate, float bevangle, int vlimit, List<HullTriangle> tris) | ||
1462 | { | ||
1463 | verts_out = null; | ||
1464 | faces_out = null; | ||
1465 | |||
1466 | if (verts.Count == 0) | ||
1467 | return 0; | ||
1468 | List<Plane> planes = new List<Plane>(); | ||
1469 | int rc = calchullpbev(verts, vlimit, out planes, bevangle, tris); | ||
1470 | if (rc == 0) | ||
1471 | return 0; | ||
1472 | return overhull(planes, verts, maxplanes, out verts_out, out faces_out, inflate); | ||
1473 | } | ||
1474 | |||
1475 | public static void addPoint(ref uint vcount, List<float3> p, float x, float y, float z) | ||
1476 | { | ||
1477 | p.Add(new float3(x, y, z)); | ||
1478 | vcount++; | ||
1479 | } | ||
1480 | |||
1481 | public static bool ComputeHull(List<float3> vertices, ref PHullResult result, int vlimit, float inflate) | ||
1482 | { | ||
1483 | List<HullTriangle> tris = new List<HullTriangle>(); | ||
1484 | List<int> faces; | ||
1485 | List<float3> verts_out; | ||
1486 | |||
1487 | if (inflate == 0.0f) | ||
1488 | { | ||
1489 | List<int> tris_out; | ||
1490 | bool ret = calchull(vertices, out tris_out, vlimit, tris); | ||
1491 | if (ret == false) | ||
1492 | return false; | ||
1493 | |||
1494 | result.Indices = tris_out; | ||
1495 | result.Vertices = vertices; | ||
1496 | return true; | ||
1497 | } | ||
1498 | else | ||
1499 | { | ||
1500 | int ret = overhullv(vertices, 35, out verts_out, out faces, inflate, 120.0f, vlimit, tris); | ||
1501 | if (ret == 0) | ||
1502 | return false; | ||
1503 | |||
1504 | List<int3> tris2 = new List<int3>(); | ||
1505 | int n = faces[0]; | ||
1506 | int k = 1; | ||
1507 | for (int i = 0; i < n; i++) | ||
1508 | { | ||
1509 | int pn = faces[k++]; | ||
1510 | for (int j = 2; j < pn; j++) | ||
1511 | tris2.Add(new int3(faces[k], faces[k + j - 1], faces[k + j])); | ||
1512 | k += pn; | ||
1513 | } | ||
1514 | Debug.Assert(tris2.Count == faces.Count - 1 - (n * 3)); | ||
1515 | |||
1516 | result.Indices = new List<int>(tris2.Count * 3); | ||
1517 | for (int i = 0; i < tris2.Count; i++) | ||
1518 | { | ||
1519 | result.Indices.Add(tris2[i].x); | ||
1520 | result.Indices.Add(tris2[i].y); | ||
1521 | result.Indices.Add(tris2[i].z); | ||
1522 | } | ||
1523 | result.Vertices = verts_out; | ||
1524 | |||
1525 | return true; | ||
1526 | } | ||
1527 | } | ||
1528 | |||
1529 | private static bool CleanupVertices(List<float3> svertices, out List<float3> vertices, float normalepsilon, out float3 scale) | ||
1530 | { | ||
1531 | const float EPSILON = 0.000001f; | ||
1532 | |||
1533 | vertices = new List<float3>(); | ||
1534 | scale = new float3(1f, 1f, 1f); | ||
1535 | |||
1536 | if (svertices.Count == 0) | ||
1537 | return false; | ||
1538 | |||
1539 | uint vcount = 0; | ||
1540 | |||
1541 | float[] recip = new float[3]; | ||
1542 | |||
1543 | float[] bmin = { Single.MaxValue, Single.MaxValue, Single.MaxValue }; | ||
1544 | float[] bmax = { Single.MinValue, Single.MinValue, Single.MinValue }; | ||
1545 | |||
1546 | for (int i = 0; i < svertices.Count; i++) | ||
1547 | { | ||
1548 | float3 p = svertices[i]; | ||
1549 | |||
1550 | for (int j = 0; j < 3; j++) | ||
1551 | { | ||
1552 | if (p[j] < bmin[j]) | ||
1553 | bmin[j] = p[j]; | ||
1554 | if (p[j] > bmax[j]) | ||
1555 | bmax[j] = p[j]; | ||
1556 | } | ||
1557 | } | ||
1558 | |||
1559 | float dx = bmax[0] - bmin[0]; | ||
1560 | float dy = bmax[1] - bmin[1]; | ||
1561 | float dz = bmax[2] - bmin[2]; | ||
1562 | |||
1563 | float3 center = new float3(); | ||
1564 | |||
1565 | center.x = dx * 0.5f + bmin[0]; | ||
1566 | center.y = dy * 0.5f + bmin[1]; | ||
1567 | center.z = dz * 0.5f + bmin[2]; | ||
1568 | |||
1569 | if (dx < EPSILON || dy < EPSILON || dz < EPSILON || svertices.Count < 3) | ||
1570 | { | ||
1571 | float len = Single.MaxValue; | ||
1572 | |||
1573 | if (dx > EPSILON && dx < len) | ||
1574 | len = dx; | ||
1575 | if (dy > EPSILON && dy < len) | ||
1576 | len = dy; | ||
1577 | if (dz > EPSILON && dz < len) | ||
1578 | len = dz; | ||
1579 | |||
1580 | if (len == Single.MaxValue) | ||
1581 | { | ||
1582 | dx = dy = dz = 0.01f; // one centimeter | ||
1583 | } | ||
1584 | else | ||
1585 | { | ||
1586 | if (dx < EPSILON) // 1/5th the shortest non-zero edge. | ||
1587 | dx = len * 0.05f; | ||
1588 | if (dy < EPSILON) | ||
1589 | dy = len * 0.05f; | ||
1590 | if (dz < EPSILON) | ||
1591 | dz = len * 0.05f; | ||
1592 | } | ||
1593 | |||
1594 | float x1 = center[0] - dx; | ||
1595 | float x2 = center[0] + dx; | ||
1596 | |||
1597 | float y1 = center[1] - dy; | ||
1598 | float y2 = center[1] + dy; | ||
1599 | |||
1600 | float z1 = center[2] - dz; | ||
1601 | float z2 = center[2] + dz; | ||
1602 | |||
1603 | addPoint(ref vcount, vertices, x1, y1, z1); | ||
1604 | addPoint(ref vcount, vertices, x2, y1, z1); | ||
1605 | addPoint(ref vcount, vertices, x2, y2, z1); | ||
1606 | addPoint(ref vcount, vertices, x1, y2, z1); | ||
1607 | addPoint(ref vcount, vertices, x1, y1, z2); | ||
1608 | addPoint(ref vcount, vertices, x2, y1, z2); | ||
1609 | addPoint(ref vcount, vertices, x2, y2, z2); | ||
1610 | addPoint(ref vcount, vertices, x1, y2, z2); | ||
1611 | |||
1612 | return true; // return cube | ||
1613 | } | ||
1614 | else | ||
1615 | { | ||
1616 | scale.x = dx; | ||
1617 | scale.y = dy; | ||
1618 | scale.z = dz; | ||
1619 | |||
1620 | recip[0] = 1f / dx; | ||
1621 | recip[1] = 1f / dy; | ||
1622 | recip[2] = 1f / dz; | ||
1623 | |||
1624 | center.x *= recip[0]; | ||
1625 | center.y *= recip[1]; | ||
1626 | center.z *= recip[2]; | ||
1627 | } | ||
1628 | |||
1629 | for (int i = 0; i < svertices.Count; i++) | ||
1630 | { | ||
1631 | float3 p = svertices[i]; | ||
1632 | |||
1633 | float px = p[0]; | ||
1634 | float py = p[1]; | ||
1635 | float pz = p[2]; | ||
1636 | |||
1637 | px = px * recip[0]; // normalize | ||
1638 | py = py * recip[1]; // normalize | ||
1639 | pz = pz * recip[2]; // normalize | ||
1640 | |||
1641 | if (true) | ||
1642 | { | ||
1643 | int j; | ||
1644 | |||
1645 | for (j = 0; j < vcount; j++) | ||
1646 | { | ||
1647 | float3 v = vertices[j]; | ||
1648 | |||
1649 | float x = v[0]; | ||
1650 | float y = v[1]; | ||
1651 | float z = v[2]; | ||
1652 | |||
1653 | float dx1 = Math.Abs(x - px); | ||
1654 | float dy1 = Math.Abs(y - py); | ||
1655 | float dz1 = Math.Abs(z - pz); | ||
1656 | |||
1657 | if (dx1 < normalepsilon && dy1 < normalepsilon && dz1 < normalepsilon) | ||
1658 | { | ||
1659 | // ok, it is close enough to the old one | ||
1660 | // now let us see if it is further from the center of the point cloud than the one we already recorded. | ||
1661 | // in which case we keep this one instead. | ||
1662 | float dist1 = GetDist(px, py, pz, center); | ||
1663 | float dist2 = GetDist(v[0], v[1], v[2], center); | ||
1664 | |||
1665 | if (dist1 > dist2) | ||
1666 | { | ||
1667 | v.x = px; | ||
1668 | v.y = py; | ||
1669 | v.z = pz; | ||
1670 | } | ||
1671 | |||
1672 | break; | ||
1673 | } | ||
1674 | } | ||
1675 | |||
1676 | if (j == vcount) | ||
1677 | { | ||
1678 | float3 dest = new float3(px, py, pz); | ||
1679 | vertices.Add(dest); | ||
1680 | vcount++; | ||
1681 | } | ||
1682 | } | ||
1683 | } | ||
1684 | |||
1685 | // ok..now make sure we didn't prune so many vertices it is now invalid. | ||
1686 | if (true) | ||
1687 | { | ||
1688 | float[] bmin2 = { Single.MaxValue, Single.MaxValue, Single.MaxValue }; | ||
1689 | float[] bmax2 = { Single.MinValue, Single.MinValue, Single.MinValue }; | ||
1690 | |||
1691 | for (int i = 0; i < vcount; i++) | ||
1692 | { | ||
1693 | float3 p = vertices[i]; | ||
1694 | for (int j = 0; j < 3; j++) | ||
1695 | { | ||
1696 | if (p[j] < bmin2[j]) | ||
1697 | bmin2[j] = p[j]; | ||
1698 | if (p[j] > bmax2[j]) | ||
1699 | bmax2[j] = p[j]; | ||
1700 | } | ||
1701 | } | ||
1702 | |||
1703 | float dx2 = bmax2[0] - bmin2[0]; | ||
1704 | float dy2 = bmax2[1] - bmin2[1]; | ||
1705 | float dz2 = bmax2[2] - bmin2[2]; | ||
1706 | |||
1707 | if (dx2 < EPSILON || dy2 < EPSILON || dz2 < EPSILON || vcount < 3) | ||
1708 | { | ||
1709 | float cx = dx2 * 0.5f + bmin2[0]; | ||
1710 | float cy = dy2 * 0.5f + bmin2[1]; | ||
1711 | float cz = dz2 * 0.5f + bmin2[2]; | ||
1712 | |||
1713 | float len = Single.MaxValue; | ||
1714 | |||
1715 | if (dx2 >= EPSILON && dx2 < len) | ||
1716 | len = dx2; | ||
1717 | if (dy2 >= EPSILON && dy2 < len) | ||
1718 | len = dy2; | ||
1719 | if (dz2 >= EPSILON && dz2 < len) | ||
1720 | len = dz2; | ||
1721 | |||
1722 | if (len == Single.MaxValue) | ||
1723 | { | ||
1724 | dx2 = dy2 = dz2 = 0.01f; // one centimeter | ||
1725 | } | ||
1726 | else | ||
1727 | { | ||
1728 | if (dx2 < EPSILON) // 1/5th the shortest non-zero edge. | ||
1729 | dx2 = len * 0.05f; | ||
1730 | if (dy2 < EPSILON) | ||
1731 | dy2 = len * 0.05f; | ||
1732 | if (dz2 < EPSILON) | ||
1733 | dz2 = len * 0.05f; | ||
1734 | } | ||
1735 | |||
1736 | float x1 = cx - dx2; | ||
1737 | float x2 = cx + dx2; | ||
1738 | |||
1739 | float y1 = cy - dy2; | ||
1740 | float y2 = cy + dy2; | ||
1741 | |||
1742 | float z1 = cz - dz2; | ||
1743 | float z2 = cz + dz2; | ||
1744 | |||
1745 | vcount = 0; // add box | ||
1746 | |||
1747 | addPoint(ref vcount, vertices, x1, y1, z1); | ||
1748 | addPoint(ref vcount, vertices, x2, y1, z1); | ||
1749 | addPoint(ref vcount, vertices, x2, y2, z1); | ||
1750 | addPoint(ref vcount, vertices, x1, y2, z1); | ||
1751 | addPoint(ref vcount, vertices, x1, y1, z2); | ||
1752 | addPoint(ref vcount, vertices, x2, y1, z2); | ||
1753 | addPoint(ref vcount, vertices, x2, y2, z2); | ||
1754 | addPoint(ref vcount, vertices, x1, y2, z2); | ||
1755 | |||
1756 | return true; | ||
1757 | } | ||
1758 | } | ||
1759 | |||
1760 | return true; | ||
1761 | } | ||
1762 | |||
1763 | private static void BringOutYourDead(List<float3> verts, out List<float3> overts, List<int> indices) | ||
1764 | { | ||
1765 | int[] used = new int[verts.Count]; | ||
1766 | int ocount = 0; | ||
1767 | |||
1768 | overts = new List<float3>(); | ||
1769 | |||
1770 | for (int i = 0; i < indices.Count; i++) | ||
1771 | { | ||
1772 | int v = indices[i]; // original array index | ||
1773 | |||
1774 | Debug.Assert(v >= 0 && v < verts.Count); | ||
1775 | |||
1776 | if (used[v] != 0) // if already remapped | ||
1777 | { | ||
1778 | indices[i] = used[v] - 1; // index to new array | ||
1779 | } | ||
1780 | else | ||
1781 | { | ||
1782 | indices[i] = ocount; // new index mapping | ||
1783 | |||
1784 | overts.Add(verts[v]); // copy old vert to new vert array | ||
1785 | |||
1786 | ocount++; // increment output vert count | ||
1787 | |||
1788 | Debug.Assert(ocount >= 0 && ocount <= verts.Count); | ||
1789 | |||
1790 | used[v] = ocount; // assign new index remapping | ||
1791 | } | ||
1792 | } | ||
1793 | } | ||
1794 | |||
1795 | public static HullError CreateConvexHull(HullDesc desc, ref HullResult result) | ||
1796 | { | ||
1797 | HullError ret = HullError.QE_FAIL; | ||
1798 | |||
1799 | PHullResult hr = new PHullResult(); | ||
1800 | |||
1801 | uint vcount = (uint)desc.Vertices.Count; | ||
1802 | if (vcount < 8) | ||
1803 | vcount = 8; | ||
1804 | |||
1805 | List<float3> vsource; | ||
1806 | float3 scale = new float3(); | ||
1807 | |||
1808 | bool ok = CleanupVertices(desc.Vertices, out vsource, desc.NormalEpsilon, out scale); // normalize point cloud, remove duplicates! | ||
1809 | |||
1810 | if (ok) | ||
1811 | { | ||
1812 | if (true) // scale vertices back to their original size. | ||
1813 | { | ||
1814 | for (int i = 0; i < vsource.Count; i++) | ||
1815 | { | ||
1816 | float3 v = vsource[i]; | ||
1817 | v.x *= scale[0]; | ||
1818 | v.y *= scale[1]; | ||
1819 | v.z *= scale[2]; | ||
1820 | } | ||
1821 | } | ||
1822 | |||
1823 | float skinwidth = 0; | ||
1824 | if (desc.HasHullFlag(HullFlag.QF_SKIN_WIDTH)) | ||
1825 | skinwidth = desc.SkinWidth; | ||
1826 | |||
1827 | ok = ComputeHull(vsource, ref hr, (int)desc.MaxVertices, skinwidth); | ||
1828 | |||
1829 | if (ok) | ||
1830 | { | ||
1831 | List<float3> vscratch; | ||
1832 | BringOutYourDead(hr.Vertices, out vscratch, hr.Indices); | ||
1833 | |||
1834 | ret = HullError.QE_OK; | ||
1835 | |||
1836 | if (desc.HasHullFlag(HullFlag.QF_TRIANGLES)) // if he wants the results as triangle! | ||
1837 | { | ||
1838 | result.Polygons = false; | ||
1839 | result.Indices = hr.Indices; | ||
1840 | result.OutputVertices = vscratch; | ||
1841 | } | ||
1842 | else | ||
1843 | { | ||
1844 | result.Polygons = true; | ||
1845 | result.OutputVertices = vscratch; | ||
1846 | |||
1847 | if (true) | ||
1848 | { | ||
1849 | List<int> source = hr.Indices; | ||
1850 | List<int> dest = new List<int>(); | ||
1851 | for (int i = 0; i < hr.Indices.Count / 3; i++) | ||
1852 | { | ||
1853 | dest.Add(3); | ||
1854 | dest.Add(source[i * 3 + 0]); | ||
1855 | dest.Add(source[i * 3 + 1]); | ||
1856 | dest.Add(source[i * 3 + 2]); | ||
1857 | } | ||
1858 | |||
1859 | result.Indices = dest; | ||
1860 | } | ||
1861 | } | ||
1862 | } | ||
1863 | } | ||
1864 | |||
1865 | return ret; | ||
1866 | } | ||
1867 | } | ||
1868 | } | ||
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/LICENSE.txt b/OpenSim/Region/Physics/ConvexDecompositionDotNet/LICENSE.txt new file mode 100644 index 0000000..714ae89 --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/LICENSE.txt | |||
@@ -0,0 +1,28 @@ | |||
1 | ConvexDecompositionDotNet | ||
2 | ------------------------- | ||
3 | |||
4 | The MIT License | ||
5 | |||
6 | Copyright (c) 2010 Intel Corporation. | ||
7 | All rights reserved. | ||
8 | |||
9 | Based on the convexdecomposition library from | ||
10 | <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
11 | |||
12 | Permission is hereby granted, free of charge, to any person obtaining a copy | ||
13 | of this software and associated documentation files (the "Software"), to deal | ||
14 | in the Software without restriction, including without limitation the rights | ||
15 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
16 | copies of the Software, and to permit persons to whom the Software is | ||
17 | furnished to do so, subject to the following conditions: | ||
18 | |||
19 | The above copyright notice and this permission notice shall be included in | ||
20 | all copies or substantial portions of the Software. | ||
21 | |||
22 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
23 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
24 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
25 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
26 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
27 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
28 | THE SOFTWARE. | ||
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/Plane.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/Plane.cs new file mode 100644 index 0000000..d099676 --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/Plane.cs | |||
@@ -0,0 +1,99 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | |||
30 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
31 | { | ||
32 | public class Plane | ||
33 | { | ||
34 | public float3 normal = new float3(); | ||
35 | public float dist; // distance below origin - the D from plane equasion Ax+By+Cz+D=0 | ||
36 | |||
37 | public Plane(float3 n, float d) | ||
38 | { | ||
39 | normal = new float3(n); | ||
40 | dist = d; | ||
41 | } | ||
42 | |||
43 | public Plane(Plane p) | ||
44 | { | ||
45 | normal = new float3(p.normal); | ||
46 | dist = p.dist; | ||
47 | } | ||
48 | |||
49 | public Plane() | ||
50 | { | ||
51 | dist = 0; | ||
52 | } | ||
53 | |||
54 | public void Transform(float3 position, Quaternion orientation) | ||
55 | { | ||
56 | // Transforms the plane to the space defined by the | ||
57 | // given position/orientation | ||
58 | float3 newNormal = Quaternion.Inverse(orientation) * normal; | ||
59 | float3 origin = Quaternion.Inverse(orientation) * (-normal * dist - position); | ||
60 | |||
61 | normal = newNormal; | ||
62 | dist = -float3.dot(newNormal, origin); | ||
63 | } | ||
64 | |||
65 | public override int GetHashCode() | ||
66 | { | ||
67 | return normal.GetHashCode() ^ dist.GetHashCode(); | ||
68 | } | ||
69 | |||
70 | public override bool Equals(object obj) | ||
71 | { | ||
72 | Plane p = obj as Plane; | ||
73 | if (p == null) | ||
74 | return false; | ||
75 | |||
76 | return this == p; | ||
77 | } | ||
78 | |||
79 | public static bool operator ==(Plane a, Plane b) | ||
80 | { | ||
81 | return (a.normal == b.normal && a.dist == b.dist); | ||
82 | } | ||
83 | |||
84 | public static bool operator !=(Plane a, Plane b) | ||
85 | { | ||
86 | return !(a == b); | ||
87 | } | ||
88 | |||
89 | public static Plane PlaneFlip(Plane plane) | ||
90 | { | ||
91 | return new Plane(-plane.normal, -plane.dist); | ||
92 | } | ||
93 | |||
94 | public static bool coplanar(Plane a, Plane b) | ||
95 | { | ||
96 | return (a == b || a == PlaneFlip(b)); | ||
97 | } | ||
98 | } | ||
99 | } | ||
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/PlaneTri.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/PlaneTri.cs new file mode 100644 index 0000000..31f0182 --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/PlaneTri.cs | |||
@@ -0,0 +1,211 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | using System.Collections.Generic; | ||
30 | using System.Diagnostics; | ||
31 | |||
32 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
33 | { | ||
34 | public enum PlaneTriResult : int | ||
35 | { | ||
36 | PTR_FRONT, | ||
37 | PTR_BACK, | ||
38 | PTR_SPLIT | ||
39 | } | ||
40 | |||
41 | public static class PlaneTri | ||
42 | { | ||
43 | private static float DistToPt(float3 p, float4 plane) | ||
44 | { | ||
45 | return p.x * plane.x + p.y * plane.y + p.z * plane.z + plane.w; | ||
46 | } | ||
47 | |||
48 | private static PlaneTriResult getSidePlane(float3 p, float4 plane, float epsilon) | ||
49 | { | ||
50 | float d = DistToPt(p, plane); | ||
51 | |||
52 | if ((d + epsilon) > 0f) | ||
53 | return PlaneTriResult.PTR_FRONT; // it is 'in front' within the provided epsilon value. | ||
54 | |||
55 | return PlaneTriResult.PTR_BACK; | ||
56 | } | ||
57 | |||
58 | private static void add(float3 p, float3[] dest, ref int pcount) | ||
59 | { | ||
60 | dest[pcount++] = new float3(p); | ||
61 | Debug.Assert(pcount <= 4); | ||
62 | } | ||
63 | |||
64 | // assumes that the points are on opposite sides of the plane! | ||
65 | private static void intersect(float3 p1, float3 p2, float3 split, float4 plane) | ||
66 | { | ||
67 | float dp1 = DistToPt(p1, plane); | ||
68 | float[] dir = new float[3]; | ||
69 | |||
70 | dir[0] = p2[0] - p1[0]; | ||
71 | dir[1] = p2[1] - p1[1]; | ||
72 | dir[2] = p2[2] - p1[2]; | ||
73 | |||
74 | float dot1 = dir[0] * plane[0] + dir[1] * plane[1] + dir[2] * plane[2]; | ||
75 | float dot2 = dp1 - plane[3]; | ||
76 | |||
77 | float t = -(plane[3] + dot2) / dot1; | ||
78 | |||
79 | split.x = (dir[0] * t) + p1[0]; | ||
80 | split.y = (dir[1] * t) + p1[1]; | ||
81 | split.z = (dir[2] * t) + p1[2]; | ||
82 | } | ||
83 | |||
84 | public static PlaneTriResult planeTriIntersection(float4 plane, FaceTri triangle, float epsilon, ref float3[] front, out int fcount, ref float3[] back, out int bcount) | ||
85 | { | ||
86 | fcount = 0; | ||
87 | bcount = 0; | ||
88 | |||
89 | // get the three vertices of the triangle. | ||
90 | float3 p1 = triangle.P1; | ||
91 | float3 p2 = triangle.P2; | ||
92 | float3 p3 = triangle.P3; | ||
93 | |||
94 | PlaneTriResult r1 = getSidePlane(p1, plane, epsilon); // compute the side of the plane each vertex is on | ||
95 | PlaneTriResult r2 = getSidePlane(p2, plane, epsilon); | ||
96 | PlaneTriResult r3 = getSidePlane(p3, plane, epsilon); | ||
97 | |||
98 | if (r1 == r2 && r1 == r3) // if all three vertices are on the same side of the plane. | ||
99 | { | ||
100 | if (r1 == PlaneTriResult.PTR_FRONT) // if all three are in front of the plane, then copy to the 'front' output triangle. | ||
101 | { | ||
102 | add(p1, front, ref fcount); | ||
103 | add(p2, front, ref fcount); | ||
104 | add(p3, front, ref fcount); | ||
105 | } | ||
106 | else | ||
107 | { | ||
108 | add(p1, back, ref bcount); // if all three are in 'back' then copy to the 'back' output triangle. | ||
109 | add(p2, back, ref bcount); | ||
110 | add(p3, back, ref bcount); | ||
111 | } | ||
112 | return r1; // if all three points are on the same side of the plane return result | ||
113 | } | ||
114 | |||
115 | // ok.. we need to split the triangle at the plane. | ||
116 | |||
117 | // First test ray segment P1 to P2 | ||
118 | if (r1 == r2) // if these are both on the same side... | ||
119 | { | ||
120 | if (r1 == PlaneTriResult.PTR_FRONT) | ||
121 | { | ||
122 | add(p1, front, ref fcount); | ||
123 | add(p2, front, ref fcount); | ||
124 | } | ||
125 | else | ||
126 | { | ||
127 | add(p1, back, ref bcount); | ||
128 | add(p2, back, ref bcount); | ||
129 | } | ||
130 | } | ||
131 | else | ||
132 | { | ||
133 | float3 split = new float3(); | ||
134 | intersect(p1, p2, split, plane); | ||
135 | |||
136 | if (r1 == PlaneTriResult.PTR_FRONT) | ||
137 | { | ||
138 | |||
139 | add(p1, front, ref fcount); | ||
140 | add(split, front, ref fcount); | ||
141 | |||
142 | add(split, back, ref bcount); | ||
143 | add(p2, back, ref bcount); | ||
144 | |||
145 | } | ||
146 | else | ||
147 | { | ||
148 | add(p1, back, ref bcount); | ||
149 | add(split, back, ref bcount); | ||
150 | |||
151 | add(split, front, ref fcount); | ||
152 | add(p2, front, ref fcount); | ||
153 | } | ||
154 | |||
155 | } | ||
156 | |||
157 | // Next test ray segment P2 to P3 | ||
158 | if (r2 == r3) // if these are both on the same side... | ||
159 | { | ||
160 | if (r3 == PlaneTriResult.PTR_FRONT) | ||
161 | { | ||
162 | add(p3, front, ref fcount); | ||
163 | } | ||
164 | else | ||
165 | { | ||
166 | add(p3, back, ref bcount); | ||
167 | } | ||
168 | } | ||
169 | else | ||
170 | { | ||
171 | float3 split = new float3(); // split the point | ||
172 | intersect(p2, p3, split, plane); | ||
173 | |||
174 | if (r3 == PlaneTriResult.PTR_FRONT) | ||
175 | { | ||
176 | add(split, front, ref fcount); | ||
177 | add(split, back, ref bcount); | ||
178 | |||
179 | add(p3, front, ref fcount); | ||
180 | } | ||
181 | else | ||
182 | { | ||
183 | add(split, front, ref fcount); | ||
184 | add(split, back, ref bcount); | ||
185 | |||
186 | add(p3, back, ref bcount); | ||
187 | } | ||
188 | } | ||
189 | |||
190 | // Next test ray segment P3 to P1 | ||
191 | if (r3 != r1) // if these are both on the same side... | ||
192 | { | ||
193 | float3 split = new float3(); // split the point | ||
194 | intersect(p3, p1, split, plane); | ||
195 | |||
196 | if (r1 == PlaneTriResult.PTR_FRONT) | ||
197 | { | ||
198 | add(split, front, ref fcount); | ||
199 | add(split, back, ref bcount); | ||
200 | } | ||
201 | else | ||
202 | { | ||
203 | add(split, front, ref fcount); | ||
204 | add(split, back, ref bcount); | ||
205 | } | ||
206 | } | ||
207 | |||
208 | return PlaneTriResult.PTR_SPLIT; | ||
209 | } | ||
210 | } | ||
211 | } | ||
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/Properties/AssemblyInfo.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/Properties/AssemblyInfo.cs new file mode 100644 index 0000000..4285e8c --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/Properties/AssemblyInfo.cs | |||
@@ -0,0 +1,36 @@ | |||
1 | using System.Reflection; | ||
2 | using System.Runtime.CompilerServices; | ||
3 | using System.Runtime.InteropServices; | ||
4 | |||
5 | // General Information about an assembly is controlled through the following | ||
6 | // set of attributes. Change these attribute values to modify the information | ||
7 | // associated with an assembly. | ||
8 | [assembly: AssemblyTitle("ConvexDecompositionDotNet")] | ||
9 | [assembly: AssemblyDescription("")] | ||
10 | [assembly: AssemblyConfiguration("")] | ||
11 | [assembly: AssemblyCompany("Intel Corporation")] | ||
12 | [assembly: AssemblyProduct("ConvexDecompositionDotNet")] | ||
13 | [assembly: AssemblyCopyright("Copyright © Intel Corporation 2010")] | ||
14 | [assembly: AssemblyTrademark("")] | ||
15 | [assembly: AssemblyCulture("")] | ||
16 | |||
17 | // Setting ComVisible to false makes the types in this assembly not visible | ||
18 | // to COM components. If you need to access a type in this assembly from | ||
19 | // COM, set the ComVisible attribute to true on that type. | ||
20 | [assembly: ComVisible(false)] | ||
21 | |||
22 | // The following GUID is for the ID of the typelib if this project is exposed to COM | ||
23 | [assembly: Guid("2a1c9467-1a17-4c8d-bf9f-4b4d86dd0cbb")] | ||
24 | |||
25 | // Version information for an assembly consists of the following four values: | ||
26 | // | ||
27 | // Major Version | ||
28 | // Minor Version | ||
29 | // Build Number | ||
30 | // Revision | ||
31 | // | ||
32 | // You can specify all the values or you can default the Build and Revision Numbers | ||
33 | // by using the '*' as shown below: | ||
34 | // [assembly: AssemblyVersion("1.0.*")] | ||
35 | [assembly: AssemblyVersion("1.0.0.0")] | ||
36 | [assembly: AssemblyFileVersion("1.0.0.0")] | ||
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/Quaternion.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/Quaternion.cs new file mode 100644 index 0000000..0ba8f17 --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/Quaternion.cs | |||
@@ -0,0 +1,209 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | |||
30 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
31 | { | ||
32 | public class Quaternion : float4 | ||
33 | { | ||
34 | public Quaternion() | ||
35 | { | ||
36 | x = y = z = 0.0f; | ||
37 | w = 1.0f; | ||
38 | } | ||
39 | |||
40 | public Quaternion(float3 v, float t) | ||
41 | { | ||
42 | v = float3.normalize(v); | ||
43 | w = (float)Math.Cos(t / 2.0f); | ||
44 | v = v * (float)Math.Sin(t / 2.0f); | ||
45 | x = v.x; | ||
46 | y = v.y; | ||
47 | z = v.z; | ||
48 | } | ||
49 | |||
50 | public Quaternion(float _x, float _y, float _z, float _w) | ||
51 | { | ||
52 | x = _x; | ||
53 | y = _y; | ||
54 | z = _z; | ||
55 | w = _w; | ||
56 | } | ||
57 | |||
58 | public float angle() | ||
59 | { | ||
60 | return (float)Math.Acos(w) * 2.0f; | ||
61 | } | ||
62 | |||
63 | public float3 axis() | ||
64 | { | ||
65 | float3 a = new float3(x, y, z); | ||
66 | if (Math.Abs(angle()) < 0.0000001f) | ||
67 | return new float3(1f, 0f, 0f); | ||
68 | return a * (1 / (float)Math.Sin(angle() / 2.0f)); | ||
69 | } | ||
70 | |||
71 | public float3 xdir() | ||
72 | { | ||
73 | return new float3(1 - 2 * (y * y + z * z), 2 * (x * y + w * z), 2 * (x * z - w * y)); | ||
74 | } | ||
75 | |||
76 | public float3 ydir() | ||
77 | { | ||
78 | return new float3(2 * (x * y - w * z), 1 - 2 * (x * x + z * z), 2 * (y * z + w * x)); | ||
79 | } | ||
80 | |||
81 | public float3 zdir() | ||
82 | { | ||
83 | return new float3(2 * (x * z + w * y), 2 * (y * z - w * x), 1 - 2 * (x * x + y * y)); | ||
84 | } | ||
85 | |||
86 | public float3x3 getmatrix() | ||
87 | { | ||
88 | return new float3x3(xdir(), ydir(), zdir()); | ||
89 | } | ||
90 | |||
91 | public static implicit operator float3x3(Quaternion q) | ||
92 | { | ||
93 | return q.getmatrix(); | ||
94 | } | ||
95 | |||
96 | public static Quaternion operator *(Quaternion a, Quaternion b) | ||
97 | { | ||
98 | Quaternion c = new Quaternion(); | ||
99 | c.w = a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z; | ||
100 | c.x = a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y; | ||
101 | c.y = a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x; | ||
102 | c.z = a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w; | ||
103 | return c; | ||
104 | } | ||
105 | |||
106 | public static float3 operator *(Quaternion q, float3 v) | ||
107 | { | ||
108 | // The following is equivalent to: | ||
109 | //return (q.getmatrix() * v); | ||
110 | float qx2 = q.x * q.x; | ||
111 | float qy2 = q.y * q.y; | ||
112 | float qz2 = q.z * q.z; | ||
113 | |||
114 | float qxqy = q.x * q.y; | ||
115 | float qxqz = q.x * q.z; | ||
116 | float qxqw = q.x * q.w; | ||
117 | float qyqz = q.y * q.z; | ||
118 | float qyqw = q.y * q.w; | ||
119 | float qzqw = q.z * q.w; | ||
120 | return new float3((1 - 2 * (qy2 + qz2)) * v.x + (2 * (qxqy - qzqw)) * v.y + (2 * (qxqz + qyqw)) * v.z, (2 * (qxqy + qzqw)) * v.x + (1 - 2 * (qx2 + qz2)) * v.y + (2 * (qyqz - qxqw)) * v.z, (2 * (qxqz - qyqw)) * v.x + (2 * (qyqz + qxqw)) * v.y + (1 - 2 * (qx2 + qy2)) * v.z); | ||
121 | } | ||
122 | |||
123 | public static Quaternion operator +(Quaternion a, Quaternion b) | ||
124 | { | ||
125 | return new Quaternion(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w); | ||
126 | } | ||
127 | |||
128 | public static Quaternion operator *(Quaternion a, float b) | ||
129 | { | ||
130 | return new Quaternion(a.x *b, a.y *b, a.z *b, a.w *b); | ||
131 | } | ||
132 | |||
133 | public static Quaternion normalize(Quaternion a) | ||
134 | { | ||
135 | float m = (float)Math.Sqrt(a.w * a.w + a.x * a.x + a.y * a.y + a.z * a.z); | ||
136 | if (m < 0.000000001f) | ||
137 | { | ||
138 | a.w = 1; | ||
139 | a.x = a.y = a.z = 0; | ||
140 | return a; | ||
141 | } | ||
142 | return a * (1f / m); | ||
143 | } | ||
144 | |||
145 | public static float dot(Quaternion a, Quaternion b) | ||
146 | { | ||
147 | return (a.w * b.w + a.x * b.x + a.y * b.y + a.z * b.z); | ||
148 | } | ||
149 | |||
150 | public static Quaternion slerp(Quaternion a, Quaternion b, float interp) | ||
151 | { | ||
152 | if (dot(a, b) < 0.0) | ||
153 | { | ||
154 | a.w = -a.w; | ||
155 | a.x = -a.x; | ||
156 | a.y = -a.y; | ||
157 | a.z = -a.z; | ||
158 | } | ||
159 | float d = dot(a, b); | ||
160 | if (d >= 1.0) | ||
161 | { | ||
162 | return a; | ||
163 | } | ||
164 | float theta = (float)Math.Acos(d); | ||
165 | if (theta == 0.0f) | ||
166 | { | ||
167 | return (a); | ||
168 | } | ||
169 | return a * ((float)Math.Sin(theta - interp * theta) / (float)Math.Sin(theta)) + b * ((float)Math.Sin(interp * theta) / (float)Math.Sin(theta)); | ||
170 | } | ||
171 | |||
172 | public static Quaternion Interpolate(Quaternion q0, Quaternion q1, float alpha) | ||
173 | { | ||
174 | return slerp(q0, q1, alpha); | ||
175 | } | ||
176 | |||
177 | public static Quaternion Inverse(Quaternion q) | ||
178 | { | ||
179 | return new Quaternion(-q.x, -q.y, -q.z, q.w); | ||
180 | } | ||
181 | |||
182 | public static Quaternion YawPitchRoll(float yaw, float pitch, float roll) | ||
183 | { | ||
184 | roll *= (3.14159264f / 180.0f); | ||
185 | yaw *= (3.14159264f / 180.0f); | ||
186 | pitch *= (3.14159264f / 180.0f); | ||
187 | return new Quaternion(new float3(0.0f, 0.0f, 1.0f), yaw) * new Quaternion(new float3(1.0f, 0.0f, 0.0f), pitch) * new Quaternion(new float3(0.0f, 1.0f, 0.0f), roll); | ||
188 | } | ||
189 | |||
190 | public static float Yaw(Quaternion q) | ||
191 | { | ||
192 | float3 v = q.ydir(); | ||
193 | return (v.y == 0.0 && v.x == 0.0) ? 0.0f : (float)Math.Atan2(-v.x, v.y) * (180.0f / 3.14159264f); | ||
194 | } | ||
195 | |||
196 | public static float Pitch(Quaternion q) | ||
197 | { | ||
198 | float3 v = q.ydir(); | ||
199 | return (float)Math.Atan2(v.z, Math.Sqrt(v.x * v.x + v.y * v.y)) * (180.0f / 3.14159264f); | ||
200 | } | ||
201 | |||
202 | public static float Roll(Quaternion q) | ||
203 | { | ||
204 | q = new Quaternion(new float3(0.0f, 0.0f, 1.0f), -Yaw(q) * (3.14159264f / 180.0f)) * q; | ||
205 | q = new Quaternion(new float3(1.0f, 0.0f, 0.0f), -Pitch(q) * (3.14159264f / 180.0f)) * q; | ||
206 | return (float)Math.Atan2(-q.xdir().z, q.xdir().x) * (180.0f / 3.14159264f); | ||
207 | } | ||
208 | } | ||
209 | } | ||
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/README.txt b/OpenSim/Region/Physics/ConvexDecompositionDotNet/README.txt new file mode 100644 index 0000000..fc53ae7 --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/README.txt | |||
@@ -0,0 +1,7 @@ | |||
1 | ConvexDecompositionDotNet | ||
2 | ========================= | ||
3 | |||
4 | A C# port of the ConvexDecomposition library by John W. Ratcliff and Stan Melax. | ||
5 | The original C++ version is available at <http://codesuppository.googlecode.com/>. | ||
6 | See the blog post at <http://codesuppository.blogspot.com/2006/08/approximate-convexdecomposition.html> | ||
7 | for a thorough explanation of generating convex hulls from concave meshes. | ||
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/SplitPlane.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/SplitPlane.cs new file mode 100644 index 0000000..9f06a9a --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/SplitPlane.cs | |||
@@ -0,0 +1,265 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | using System.Collections.Generic; | ||
30 | |||
31 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
32 | { | ||
33 | public class Rect3d | ||
34 | { | ||
35 | public float[] mMin = new float[3]; | ||
36 | public float[] mMax = new float[3]; | ||
37 | |||
38 | public Rect3d() | ||
39 | { | ||
40 | } | ||
41 | |||
42 | public Rect3d(float[] bmin, float[] bmax) | ||
43 | { | ||
44 | mMin[0] = bmin[0]; | ||
45 | mMin[1] = bmin[1]; | ||
46 | mMin[2] = bmin[2]; | ||
47 | |||
48 | mMax[0] = bmax[0]; | ||
49 | mMax[1] = bmax[1]; | ||
50 | mMax[2] = bmax[2]; | ||
51 | } | ||
52 | |||
53 | public void SetMin(float[] bmin) | ||
54 | { | ||
55 | mMin[0] = bmin[0]; | ||
56 | mMin[1] = bmin[1]; | ||
57 | mMin[2] = bmin[2]; | ||
58 | } | ||
59 | |||
60 | public void SetMax(float[] bmax) | ||
61 | { | ||
62 | mMax[0] = bmax[0]; | ||
63 | mMax[1] = bmax[1]; | ||
64 | mMax[2] = bmax[2]; | ||
65 | } | ||
66 | |||
67 | public void SetMin(float x, float y, float z) | ||
68 | { | ||
69 | mMin[0] = x; | ||
70 | mMin[1] = y; | ||
71 | mMin[2] = z; | ||
72 | } | ||
73 | |||
74 | public void SetMax(float x, float y, float z) | ||
75 | { | ||
76 | mMax[0] = x; | ||
77 | mMax[1] = y; | ||
78 | mMax[2] = z; | ||
79 | } | ||
80 | } | ||
81 | |||
82 | public static class SplitPlane | ||
83 | { | ||
84 | public static bool computeSplitPlane(List<float3> vertices, List<int> indices, ref float4 plane) | ||
85 | { | ||
86 | float[] bmin = { Single.MaxValue, Single.MaxValue, Single.MaxValue }; | ||
87 | float[] bmax = { Single.MinValue, Single.MinValue, Single.MinValue }; | ||
88 | |||
89 | for (int i = 0; i < vertices.Count; i++) | ||
90 | { | ||
91 | float3 p = vertices[i]; | ||
92 | |||
93 | if (p[0] < bmin[0]) | ||
94 | bmin[0] = p[0]; | ||
95 | if (p[1] < bmin[1]) | ||
96 | bmin[1] = p[1]; | ||
97 | if (p[2] < bmin[2]) | ||
98 | bmin[2] = p[2]; | ||
99 | |||
100 | if (p[0] > bmax[0]) | ||
101 | bmax[0] = p[0]; | ||
102 | if (p[1] > bmax[1]) | ||
103 | bmax[1] = p[1]; | ||
104 | if (p[2] > bmax[2]) | ||
105 | bmax[2] = p[2]; | ||
106 | } | ||
107 | |||
108 | float dx = bmax[0] - bmin[0]; | ||
109 | float dy = bmax[1] - bmin[1]; | ||
110 | float dz = bmax[2] - bmin[2]; | ||
111 | |||
112 | float laxis = dx; | ||
113 | |||
114 | int axis = 0; | ||
115 | |||
116 | if (dy > dx) | ||
117 | { | ||
118 | axis = 1; | ||
119 | laxis = dy; | ||
120 | } | ||
121 | |||
122 | if (dz > dx && dz > dy) | ||
123 | { | ||
124 | axis = 2; | ||
125 | laxis = dz; | ||
126 | } | ||
127 | |||
128 | float[] p1 = new float[3]; | ||
129 | float[] p2 = new float[3]; | ||
130 | float[] p3 = new float[3]; | ||
131 | |||
132 | p3[0] = p2[0] = p1[0] = bmin[0] + dx * 0.5f; | ||
133 | p3[1] = p2[1] = p1[1] = bmin[1] + dy * 0.5f; | ||
134 | p3[2] = p2[2] = p1[2] = bmin[2] + dz * 0.5f; | ||
135 | |||
136 | Rect3d b = new Rect3d(bmin, bmax); | ||
137 | |||
138 | Rect3d b1 = new Rect3d(); | ||
139 | Rect3d b2 = new Rect3d(); | ||
140 | |||
141 | splitRect(axis, b, b1, b2, p1); | ||
142 | |||
143 | switch (axis) | ||
144 | { | ||
145 | case 0: | ||
146 | p2[1] = bmin[1]; | ||
147 | p2[2] = bmin[2]; | ||
148 | |||
149 | if (dz > dy) | ||
150 | { | ||
151 | p3[1] = bmax[1]; | ||
152 | p3[2] = bmin[2]; | ||
153 | } | ||
154 | else | ||
155 | { | ||
156 | p3[1] = bmin[1]; | ||
157 | p3[2] = bmax[2]; | ||
158 | } | ||
159 | |||
160 | break; | ||
161 | case 1: | ||
162 | p2[0] = bmin[0]; | ||
163 | p2[2] = bmin[2]; | ||
164 | |||
165 | if (dx > dz) | ||
166 | { | ||
167 | p3[0] = bmax[0]; | ||
168 | p3[2] = bmin[2]; | ||
169 | } | ||
170 | else | ||
171 | { | ||
172 | p3[0] = bmin[0]; | ||
173 | p3[2] = bmax[2]; | ||
174 | } | ||
175 | |||
176 | break; | ||
177 | case 2: | ||
178 | p2[0] = bmin[0]; | ||
179 | p2[1] = bmin[1]; | ||
180 | |||
181 | if (dx > dy) | ||
182 | { | ||
183 | p3[0] = bmax[0]; | ||
184 | p3[1] = bmin[1]; | ||
185 | } | ||
186 | else | ||
187 | { | ||
188 | p3[0] = bmin[0]; | ||
189 | p3[1] = bmax[1]; | ||
190 | } | ||
191 | |||
192 | break; | ||
193 | } | ||
194 | |||
195 | computePlane(p1, p2, p3, plane); | ||
196 | |||
197 | return true; | ||
198 | } | ||
199 | |||
200 | internal static void computePlane(float[] A, float[] B, float[] C, float4 plane) | ||
201 | { | ||
202 | float vx = (B[0] - C[0]); | ||
203 | float vy = (B[1] - C[1]); | ||
204 | float vz = (B[2] - C[2]); | ||
205 | |||
206 | float wx = (A[0] - B[0]); | ||
207 | float wy = (A[1] - B[1]); | ||
208 | float wz = (A[2] - B[2]); | ||
209 | |||
210 | float vw_x = vy * wz - vz * wy; | ||
211 | float vw_y = vz * wx - vx * wz; | ||
212 | float vw_z = vx * wy - vy * wx; | ||
213 | |||
214 | float mag = (float)Math.Sqrt((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z)); | ||
215 | |||
216 | if (mag < 0.000001f) | ||
217 | { | ||
218 | mag = 0; | ||
219 | } | ||
220 | else | ||
221 | { | ||
222 | mag = 1.0f / mag; | ||
223 | } | ||
224 | |||
225 | float x = vw_x * mag; | ||
226 | float y = vw_y * mag; | ||
227 | float z = vw_z * mag; | ||
228 | |||
229 | float D = 0.0f - ((x * A[0]) + (y * A[1]) + (z * A[2])); | ||
230 | |||
231 | plane.x = x; | ||
232 | plane.y = y; | ||
233 | plane.z = z; | ||
234 | plane.w = D; | ||
235 | } | ||
236 | |||
237 | public static void splitRect(int axis, Rect3d source, Rect3d b1, Rect3d b2, float[] midpoint) | ||
238 | { | ||
239 | switch (axis) | ||
240 | { | ||
241 | case 0: | ||
242 | b1.SetMin(source.mMin); | ||
243 | b1.SetMax(midpoint[0], source.mMax[1], source.mMax[2]); | ||
244 | |||
245 | b2.SetMin(midpoint[0], source.mMin[1], source.mMin[2]); | ||
246 | b2.SetMax(source.mMax); | ||
247 | break; | ||
248 | case 1: | ||
249 | b1.SetMin(source.mMin); | ||
250 | b1.SetMax(source.mMax[0], midpoint[1], source.mMax[2]); | ||
251 | |||
252 | b2.SetMin(source.mMin[0], midpoint[1], source.mMin[2]); | ||
253 | b2.SetMax(source.mMax); | ||
254 | break; | ||
255 | case 2: | ||
256 | b1.SetMin(source.mMin); | ||
257 | b1.SetMax(source.mMax[0], source.mMax[1], midpoint[2]); | ||
258 | |||
259 | b2.SetMin(source.mMin[0], source.mMin[1], midpoint[2]); | ||
260 | b2.SetMax(source.mMax); | ||
261 | break; | ||
262 | } | ||
263 | } | ||
264 | } | ||
265 | } | ||
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/VertexLookup.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/VertexLookup.cs new file mode 100644 index 0000000..6f17c9f --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/VertexLookup.cs | |||
@@ -0,0 +1,70 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | using System.Collections.Generic; | ||
30 | |||
31 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
32 | { | ||
33 | public class VertexPool | ||
34 | { | ||
35 | private List<float3> mVertices = new List<float3>(); | ||
36 | private Dictionary<float3, int> mIndices = new Dictionary<float3, int>(); | ||
37 | |||
38 | public int getIndex(float3 vtx) | ||
39 | { | ||
40 | int idx; | ||
41 | if (mIndices.TryGetValue(vtx, out idx)) | ||
42 | return idx; | ||
43 | |||
44 | idx = mVertices.Count; | ||
45 | mVertices.Add(vtx); | ||
46 | mIndices.Add(vtx, idx); | ||
47 | return idx; | ||
48 | } | ||
49 | |||
50 | public float3 Get(int idx) | ||
51 | { | ||
52 | return mVertices[idx]; | ||
53 | } | ||
54 | |||
55 | public int GetSize() | ||
56 | { | ||
57 | return mVertices.Count; | ||
58 | } | ||
59 | |||
60 | public List<float3> GetVertices() | ||
61 | { | ||
62 | return mVertices; | ||
63 | } | ||
64 | |||
65 | public void Clear() | ||
66 | { | ||
67 | mVertices.Clear(); | ||
68 | } | ||
69 | } | ||
70 | } | ||
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/float2.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float2.cs new file mode 100644 index 0000000..ce88fc8 --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float2.cs | |||
@@ -0,0 +1,70 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | |||
30 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
31 | { | ||
32 | public class float2 | ||
33 | { | ||
34 | public float x; | ||
35 | public float y; | ||
36 | |||
37 | public float2() | ||
38 | { | ||
39 | } | ||
40 | |||
41 | public float2(float _x, float _y) | ||
42 | { | ||
43 | x = _x; | ||
44 | y = _y; | ||
45 | } | ||
46 | |||
47 | public float this[int i] | ||
48 | { | ||
49 | get | ||
50 | { | ||
51 | switch (i) | ||
52 | { | ||
53 | case 0: return x; | ||
54 | case 1: return y; | ||
55 | } | ||
56 | throw new ArgumentOutOfRangeException(); | ||
57 | } | ||
58 | } | ||
59 | |||
60 | public static float2 operator -(float2 a, float2 b) | ||
61 | { | ||
62 | return new float2(a.x - b.x, a.y - b.y); | ||
63 | } | ||
64 | |||
65 | public static float2 operator +(float2 a, float2 b) | ||
66 | { | ||
67 | return new float2(a.x + b.x, a.y + b.y); | ||
68 | } | ||
69 | } | ||
70 | } | ||
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/float3.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float3.cs new file mode 100644 index 0000000..4389114 --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float3.cs | |||
@@ -0,0 +1,444 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | |||
30 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
31 | { | ||
32 | public class float3 : IEquatable<float3> | ||
33 | { | ||
34 | public float x; | ||
35 | public float y; | ||
36 | public float z; | ||
37 | |||
38 | public float3() | ||
39 | { | ||
40 | x = 0; | ||
41 | y = 0; | ||
42 | z = 0; | ||
43 | } | ||
44 | |||
45 | public float3(float _x, float _y, float _z) | ||
46 | { | ||
47 | x = _x; | ||
48 | y = _y; | ||
49 | z = _z; | ||
50 | } | ||
51 | |||
52 | public float3(float3 f) | ||
53 | { | ||
54 | x = f.x; | ||
55 | y = f.y; | ||
56 | z = f.z; | ||
57 | } | ||
58 | |||
59 | public float this[int i] | ||
60 | { | ||
61 | get | ||
62 | { | ||
63 | switch (i) | ||
64 | { | ||
65 | case 0: return x; | ||
66 | case 1: return y; | ||
67 | case 2: return z; | ||
68 | } | ||
69 | throw new ArgumentOutOfRangeException(); | ||
70 | } | ||
71 | } | ||
72 | |||
73 | public float Distance(float3 a) | ||
74 | { | ||
75 | float3 d = new float3(a.x - x, a.y - y, a.z - z); | ||
76 | return d.Length(); | ||
77 | } | ||
78 | |||
79 | public float Distance2(float3 a) | ||
80 | { | ||
81 | float dx = a.x - x; | ||
82 | float dy = a.y - y; | ||
83 | float dz = a.z - z; | ||
84 | return dx * dx + dy * dy + dz * dz; | ||
85 | } | ||
86 | |||
87 | public float Length() | ||
88 | { | ||
89 | return (float)Math.Sqrt(x * x + y * y + z * z); | ||
90 | } | ||
91 | |||
92 | public float Area(float3 p1, float3 p2) | ||
93 | { | ||
94 | float A = Partial(p1); | ||
95 | A += p1.Partial(p2); | ||
96 | A += p2.Partial(this); | ||
97 | return A * 0.5f; | ||
98 | } | ||
99 | |||
100 | public float Partial(float3 p) | ||
101 | { | ||
102 | return (x * p.y) - (p.x * y); | ||
103 | } | ||
104 | |||
105 | // Given a point and a line (defined by two points), compute the closest point | ||
106 | // in the line. (The line is treated as infinitely long.) | ||
107 | public void NearestPointInLine(float3 point, float3 line0, float3 line1) | ||
108 | { | ||
109 | float3 nearestPoint = new float3(); | ||
110 | float3 lineDelta = line1 - line0; | ||
111 | |||
112 | // Handle degenerate lines | ||
113 | if (lineDelta == float3.Zero) | ||
114 | { | ||
115 | nearestPoint = line0; | ||
116 | } | ||
117 | else | ||
118 | { | ||
119 | float delta = float3.dot(point - line0, lineDelta) / float3.dot(lineDelta, lineDelta); | ||
120 | nearestPoint = line0 + lineDelta * delta; | ||
121 | } | ||
122 | |||
123 | this.x = nearestPoint.x; | ||
124 | this.y = nearestPoint.y; | ||
125 | this.z = nearestPoint.z; | ||
126 | } | ||
127 | |||
128 | // Given a point and a line segment (defined by two points), compute the closest point | ||
129 | // in the line. Cap the point at the endpoints of the line segment. | ||
130 | public void NearestPointInLineSegment(float3 point, float3 line0, float3 line1) | ||
131 | { | ||
132 | float3 nearestPoint = new float3(); | ||
133 | float3 lineDelta = line1 - line0; | ||
134 | |||
135 | // Handle degenerate lines | ||
136 | if (lineDelta == Zero) | ||
137 | { | ||
138 | nearestPoint = line0; | ||
139 | } | ||
140 | else | ||
141 | { | ||
142 | float delta = float3.dot(point - line0, lineDelta) / float3.dot(lineDelta, lineDelta); | ||
143 | |||
144 | // Clamp the point to conform to the segment's endpoints | ||
145 | if (delta < 0) | ||
146 | delta = 0; | ||
147 | else if (delta > 1) | ||
148 | delta = 1; | ||
149 | |||
150 | nearestPoint = line0 + lineDelta * delta; | ||
151 | } | ||
152 | |||
153 | this.x = nearestPoint.x; | ||
154 | this.y = nearestPoint.y; | ||
155 | this.z = nearestPoint.z; | ||
156 | } | ||
157 | |||
158 | // Given a point and a triangle (defined by three points), compute the closest point | ||
159 | // in the triangle. Clamp the point so it's confined to the area of the triangle. | ||
160 | public void NearestPointInTriangle(float3 point, float3 triangle0, float3 triangle1, float3 triangle2) | ||
161 | { | ||
162 | float3 nearestPoint = new float3(); | ||
163 | |||
164 | float3 lineDelta0 = triangle1 - triangle0; | ||
165 | float3 lineDelta1 = triangle2 - triangle0; | ||
166 | |||
167 | // Handle degenerate triangles | ||
168 | if ((lineDelta0 == Zero) || (lineDelta1 == Zero)) | ||
169 | { | ||
170 | nearestPoint.NearestPointInLineSegment(point, triangle1, triangle2); | ||
171 | } | ||
172 | else if (lineDelta0 == lineDelta1) | ||
173 | { | ||
174 | nearestPoint.NearestPointInLineSegment(point, triangle0, triangle1); | ||
175 | } | ||
176 | else | ||
177 | { | ||
178 | float3[] axis = new float3[3] { new float3(), new float3(), new float3() }; | ||
179 | axis[0].NearestPointInLine(triangle0, triangle1, triangle2); | ||
180 | axis[1].NearestPointInLine(triangle1, triangle0, triangle2); | ||
181 | axis[2].NearestPointInLine(triangle2, triangle0, triangle1); | ||
182 | |||
183 | float3 axisDot = new float3(); | ||
184 | axisDot.x = dot(triangle0 - axis[0], point - axis[0]); | ||
185 | axisDot.y = dot(triangle1 - axis[1], point - axis[1]); | ||
186 | axisDot.z = dot(triangle2 - axis[2], point - axis[2]); | ||
187 | |||
188 | bool bForce = true; | ||
189 | float bestMagnitude2 = 0; | ||
190 | float closeMagnitude2; | ||
191 | float3 closePoint = new float3(); | ||
192 | |||
193 | if (axisDot.x < 0f) | ||
194 | { | ||
195 | closePoint.NearestPointInLineSegment(point, triangle1, triangle2); | ||
196 | closeMagnitude2 = point.Distance2(closePoint); | ||
197 | if (bForce || (bestMagnitude2 > closeMagnitude2)) | ||
198 | { | ||
199 | bForce = false; | ||
200 | bestMagnitude2 = closeMagnitude2; | ||
201 | nearestPoint = closePoint; | ||
202 | } | ||
203 | } | ||
204 | if (axisDot.y < 0f) | ||
205 | { | ||
206 | closePoint.NearestPointInLineSegment(point, triangle0, triangle2); | ||
207 | closeMagnitude2 = point.Distance2(closePoint); | ||
208 | if (bForce || (bestMagnitude2 > closeMagnitude2)) | ||
209 | { | ||
210 | bForce = false; | ||
211 | bestMagnitude2 = closeMagnitude2; | ||
212 | nearestPoint = closePoint; | ||
213 | } | ||
214 | } | ||
215 | if (axisDot.z < 0f) | ||
216 | { | ||
217 | closePoint.NearestPointInLineSegment(point, triangle0, triangle1); | ||
218 | closeMagnitude2 = point.Distance2(closePoint); | ||
219 | if (bForce || (bestMagnitude2 > closeMagnitude2)) | ||
220 | { | ||
221 | bForce = false; | ||
222 | bestMagnitude2 = closeMagnitude2; | ||
223 | nearestPoint = closePoint; | ||
224 | } | ||
225 | } | ||
226 | |||
227 | // If bForce is true at this point, it means the nearest point lies | ||
228 | // inside the triangle; use the nearest-point-on-a-plane equation | ||
229 | if (bForce) | ||
230 | { | ||
231 | float3 normal; | ||
232 | |||
233 | // Get the normal of the polygon (doesn't have to be a unit vector) | ||
234 | normal = float3.cross(lineDelta0, lineDelta1); | ||
235 | |||
236 | float3 pointDelta = point - triangle0; | ||
237 | float delta = float3.dot(normal, pointDelta) / float3.dot(normal, normal); | ||
238 | |||
239 | nearestPoint = point - normal * delta; | ||
240 | } | ||
241 | } | ||
242 | |||
243 | this.x = nearestPoint.x; | ||
244 | this.y = nearestPoint.y; | ||
245 | this.z = nearestPoint.z; | ||
246 | } | ||
247 | |||
248 | public static float3 operator +(float3 a, float3 b) | ||
249 | { | ||
250 | return new float3(a.x + b.x, a.y + b.y, a.z + b.z); | ||
251 | } | ||
252 | |||
253 | public static float3 operator -(float3 a, float3 b) | ||
254 | { | ||
255 | return new float3(a.x - b.x, a.y - b.y, a.z - b.z); | ||
256 | } | ||
257 | |||
258 | public static float3 operator -(float3 a, float s) | ||
259 | { | ||
260 | return new float3(a.x - s, a.y - s, a.z - s); | ||
261 | } | ||
262 | |||
263 | public static float3 operator -(float3 v) | ||
264 | { | ||
265 | return new float3(-v.x, -v.y, -v.z); | ||
266 | } | ||
267 | |||
268 | public static float3 operator *(float3 v, float s) | ||
269 | { | ||
270 | return new float3(v.x * s, v.y * s, v.z * s); | ||
271 | } | ||
272 | |||
273 | public static float3 operator *(float s, float3 v) | ||
274 | { | ||
275 | return new float3(v.x * s, v.y * s, v.z * s); | ||
276 | } | ||
277 | |||
278 | public static float3 operator *(float3 v, float3x3 m) | ||
279 | { | ||
280 | return new float3((m.x.x * v.x + m.y.x * v.y + m.z.x * v.z), (m.x.y * v.x + m.y.y * v.y + m.z.y * v.z), (m.x.z * v.x + m.y.z * v.y + m.z.z * v.z)); | ||
281 | } | ||
282 | |||
283 | public static float3 operator *(float3x3 m, float3 v) | ||
284 | { | ||
285 | return new float3(dot(m.x, v), dot(m.y, v), dot(m.z, v)); | ||
286 | } | ||
287 | |||
288 | public static float3 operator /(float3 v, float s) | ||
289 | { | ||
290 | float sinv = 1.0f / s; | ||
291 | return new float3(v.x * sinv, v.y * sinv, v.z * sinv); | ||
292 | } | ||
293 | |||
294 | public bool Equals(float3 other) | ||
295 | { | ||
296 | return this == other; | ||
297 | } | ||
298 | |||
299 | public override bool Equals(object obj) | ||
300 | { | ||
301 | float3 f = obj as float3; | ||
302 | if (f == null) | ||
303 | return false; | ||
304 | |||
305 | return this == f; | ||
306 | } | ||
307 | |||
308 | public override int GetHashCode() | ||
309 | { | ||
310 | return x.GetHashCode() ^ y.GetHashCode() ^ z.GetHashCode(); | ||
311 | } | ||
312 | |||
313 | public static bool operator ==(float3 a, float3 b) | ||
314 | { | ||
315 | // If both are null, or both are same instance, return true. | ||
316 | if (System.Object.ReferenceEquals(a, b)) | ||
317 | return true; | ||
318 | // If one is null, but not both, return false. | ||
319 | if (((object)a == null) || ((object)b == null)) | ||
320 | return false; | ||
321 | |||
322 | return (a.x == b.x && a.y == b.y && a.z == b.z); | ||
323 | } | ||
324 | |||
325 | public static bool operator !=(float3 a, float3 b) | ||
326 | { | ||
327 | return (a.x != b.x || a.y != b.y || a.z != b.z); | ||
328 | } | ||
329 | |||
330 | public static float dot(float3 a, float3 b) | ||
331 | { | ||
332 | return a.x * b.x + a.y * b.y + a.z * b.z; | ||
333 | } | ||
334 | |||
335 | public static float3 cmul(float3 v1, float3 v2) | ||
336 | { | ||
337 | return new float3(v1.x * v2.x, v1.y * v2.y, v1.z * v2.z); | ||
338 | } | ||
339 | |||
340 | public static float3 cross(float3 a, float3 b) | ||
341 | { | ||
342 | return new float3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x); | ||
343 | } | ||
344 | |||
345 | public static float3 Interpolate(float3 v0, float3 v1, float alpha) | ||
346 | { | ||
347 | return v0 * (1 - alpha) + v1 * alpha; | ||
348 | } | ||
349 | |||
350 | public static float3 Round(float3 a, int digits) | ||
351 | { | ||
352 | return new float3((float)Math.Round(a.x, digits), (float)Math.Round(a.y, digits), (float)Math.Round(a.z, digits)); | ||
353 | } | ||
354 | |||
355 | public static float3 VectorMax(float3 a, float3 b) | ||
356 | { | ||
357 | return new float3(Math.Max(a.x, b.x), Math.Max(a.y, b.y), Math.Max(a.z, b.z)); | ||
358 | } | ||
359 | |||
360 | public static float3 VectorMin(float3 a, float3 b) | ||
361 | { | ||
362 | return new float3(Math.Min(a.x, b.x), Math.Min(a.y, b.y), Math.Min(a.z, b.z)); | ||
363 | } | ||
364 | |||
365 | public static float3 vabs(float3 v) | ||
366 | { | ||
367 | return new float3(Math.Abs(v.x), Math.Abs(v.y), Math.Abs(v.z)); | ||
368 | } | ||
369 | |||
370 | public static float magnitude(float3 v) | ||
371 | { | ||
372 | return (float)Math.Sqrt(v.x * v.x + v.y * v.y + v.z * v.z); | ||
373 | } | ||
374 | |||
375 | public static float3 normalize(float3 v) | ||
376 | { | ||
377 | float d = magnitude(v); | ||
378 | if (d == 0) | ||
379 | d = 0.1f; | ||
380 | d = 1 / d; | ||
381 | return new float3(v.x * d, v.y * d, v.z * d); | ||
382 | } | ||
383 | |||
384 | public static float3 safenormalize(float3 v) | ||
385 | { | ||
386 | if (magnitude(v) <= 0.0f) | ||
387 | return new float3(1, 0, 0); | ||
388 | else | ||
389 | return normalize(v); | ||
390 | } | ||
391 | |||
392 | public static float Yaw(float3 v) | ||
393 | { | ||
394 | return (v.y == 0.0 && v.x == 0.0) ? 0.0f : (float)Math.Atan2(-v.x, v.y) * (180.0f / 3.14159264f); | ||
395 | } | ||
396 | |||
397 | public static float Pitch(float3 v) | ||
398 | { | ||
399 | return (float)Math.Atan2(v.z, Math.Sqrt(v.x * v.x + v.y * v.y)) * (180.0f / 3.14159264f); | ||
400 | } | ||
401 | |||
402 | public float ComputePlane(float3 A, float3 B, float3 C) | ||
403 | { | ||
404 | float vx, vy, vz, wx, wy, wz, vw_x, vw_y, vw_z, mag; | ||
405 | |||
406 | vx = (B.x - C.x); | ||
407 | vy = (B.y - C.y); | ||
408 | vz = (B.z - C.z); | ||
409 | |||
410 | wx = (A.x - B.x); | ||
411 | wy = (A.y - B.y); | ||
412 | wz = (A.z - B.z); | ||
413 | |||
414 | vw_x = vy * wz - vz * wy; | ||
415 | vw_y = vz * wx - vx * wz; | ||
416 | vw_z = vx * wy - vy * wx; | ||
417 | |||
418 | mag = (float)Math.Sqrt((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z)); | ||
419 | |||
420 | if (mag < 0.000001f) | ||
421 | { | ||
422 | mag = 0; | ||
423 | } | ||
424 | else | ||
425 | { | ||
426 | mag = 1.0f / mag; | ||
427 | } | ||
428 | |||
429 | x = vw_x * mag; | ||
430 | y = vw_y * mag; | ||
431 | z = vw_z * mag; | ||
432 | |||
433 | float D = 0.0f - ((x * A.x) + (y * A.y) + (z * A.z)); | ||
434 | return D; | ||
435 | } | ||
436 | |||
437 | public override string ToString() | ||
438 | { | ||
439 | return String.Format("<{0}, {1}, {2}>", x, y, z); | ||
440 | } | ||
441 | |||
442 | public static readonly float3 Zero = new float3(); | ||
443 | } | ||
444 | } | ||
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/float3x3.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float3x3.cs new file mode 100644 index 0000000..76cf063 --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float3x3.cs | |||
@@ -0,0 +1,195 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | using System.Collections.Generic; | ||
30 | using System.Diagnostics; | ||
31 | |||
32 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
33 | { | ||
34 | public class float3x3 | ||
35 | { | ||
36 | public float3 x = new float3(); | ||
37 | public float3 y = new float3(); | ||
38 | public float3 z = new float3(); | ||
39 | |||
40 | public float3x3() | ||
41 | { | ||
42 | } | ||
43 | |||
44 | public float3x3(float xx, float xy, float xz, float yx, float yy, float yz, float zx, float zy, float zz) | ||
45 | { | ||
46 | x = new float3(xx, xy, xz); | ||
47 | y = new float3(yx, yy, yz); | ||
48 | z = new float3(zx, zy, zz); | ||
49 | } | ||
50 | |||
51 | public float3x3(float3 _x, float3 _y, float3 _z) | ||
52 | { | ||
53 | x = new float3(_x); | ||
54 | y = new float3(_y); | ||
55 | z = new float3(_z); | ||
56 | } | ||
57 | |||
58 | public float3 this[int i] | ||
59 | { | ||
60 | get | ||
61 | { | ||
62 | switch (i) | ||
63 | { | ||
64 | case 0: return x; | ||
65 | case 1: return y; | ||
66 | case 2: return z; | ||
67 | } | ||
68 | throw new ArgumentOutOfRangeException(); | ||
69 | } | ||
70 | } | ||
71 | |||
72 | public float this[int i, int j] | ||
73 | { | ||
74 | get | ||
75 | { | ||
76 | switch (i) | ||
77 | { | ||
78 | case 0: | ||
79 | switch (j) | ||
80 | { | ||
81 | case 0: return x.x; | ||
82 | case 1: return x.y; | ||
83 | case 2: return x.z; | ||
84 | } | ||
85 | break; | ||
86 | case 1: | ||
87 | switch (j) | ||
88 | { | ||
89 | case 0: return y.x; | ||
90 | case 1: return y.y; | ||
91 | case 2: return y.z; | ||
92 | } | ||
93 | break; | ||
94 | case 2: | ||
95 | switch (j) | ||
96 | { | ||
97 | case 0: return z.x; | ||
98 | case 1: return z.y; | ||
99 | case 2: return z.z; | ||
100 | } | ||
101 | break; | ||
102 | } | ||
103 | throw new ArgumentOutOfRangeException(); | ||
104 | } | ||
105 | set | ||
106 | { | ||
107 | switch (i) | ||
108 | { | ||
109 | case 0: | ||
110 | switch (j) | ||
111 | { | ||
112 | case 0: x.x = value; return; | ||
113 | case 1: x.y = value; return; | ||
114 | case 2: x.z = value; return; | ||
115 | } | ||
116 | break; | ||
117 | case 1: | ||
118 | switch (j) | ||
119 | { | ||
120 | case 0: y.x = value; return; | ||
121 | case 1: y.y = value; return; | ||
122 | case 2: y.z = value; return; | ||
123 | } | ||
124 | break; | ||
125 | case 2: | ||
126 | switch (j) | ||
127 | { | ||
128 | case 0: z.x = value; return; | ||
129 | case 1: z.y = value; return; | ||
130 | case 2: z.z = value; return; | ||
131 | } | ||
132 | break; | ||
133 | } | ||
134 | throw new ArgumentOutOfRangeException(); | ||
135 | } | ||
136 | } | ||
137 | |||
138 | public static float3x3 Transpose(float3x3 m) | ||
139 | { | ||
140 | return new float3x3(new float3(m.x.x, m.y.x, m.z.x), new float3(m.x.y, m.y.y, m.z.y), new float3(m.x.z, m.y.z, m.z.z)); | ||
141 | } | ||
142 | |||
143 | public static float3x3 operator *(float3x3 a, float3x3 b) | ||
144 | { | ||
145 | return new float3x3(a.x * b, a.y * b, a.z * b); | ||
146 | } | ||
147 | |||
148 | public static float3x3 operator *(float3x3 a, float s) | ||
149 | { | ||
150 | return new float3x3(a.x * s, a.y * s, a.z * s); | ||
151 | } | ||
152 | |||
153 | public static float3x3 operator /(float3x3 a, float s) | ||
154 | { | ||
155 | float t = 1f / s; | ||
156 | return new float3x3(a.x * t, a.y * t, a.z * t); | ||
157 | } | ||
158 | |||
159 | public static float3x3 operator +(float3x3 a, float3x3 b) | ||
160 | { | ||
161 | return new float3x3(a.x + b.x, a.y + b.y, a.z + b.z); | ||
162 | } | ||
163 | |||
164 | public static float3x3 operator -(float3x3 a, float3x3 b) | ||
165 | { | ||
166 | return new float3x3(a.x - b.x, a.y - b.y, a.z - b.z); | ||
167 | } | ||
168 | |||
169 | public static float Determinant(float3x3 m) | ||
170 | { | ||
171 | return m.x.x * m.y.y * m.z.z + m.y.x * m.z.y * m.x.z + m.z.x * m.x.y * m.y.z - m.x.x * m.z.y * m.y.z - m.y.x * m.x.y * m.z.z - m.z.x * m.y.y * m.x.z; | ||
172 | } | ||
173 | |||
174 | public static float3x3 Inverse(float3x3 a) | ||
175 | { | ||
176 | float3x3 b = new float3x3(); | ||
177 | float d = Determinant(a); | ||
178 | Debug.Assert(d != 0); | ||
179 | for (int i = 0; i < 3; i++) | ||
180 | { | ||
181 | for (int j = 0; j < 3; j++) | ||
182 | { | ||
183 | int i1 = (i + 1) % 3; | ||
184 | int i2 = (i + 2) % 3; | ||
185 | int j1 = (j + 1) % 3; | ||
186 | int j2 = (j + 2) % 3; | ||
187 | |||
188 | // reverse indexs i&j to take transpose | ||
189 | b[i, j] = (a[i1][j1] * a[i2][j2] - a[i1][j2] * a[i2][j1]) / d; | ||
190 | } | ||
191 | } | ||
192 | return b; | ||
193 | } | ||
194 | } | ||
195 | } | ||
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/float4.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float4.cs new file mode 100644 index 0000000..fa60876 --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float4.cs | |||
@@ -0,0 +1,170 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | |||
30 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
31 | { | ||
32 | public class float4 | ||
33 | { | ||
34 | public float x; | ||
35 | public float y; | ||
36 | public float z; | ||
37 | public float w; | ||
38 | |||
39 | public float4() | ||
40 | { | ||
41 | x = 0; | ||
42 | y = 0; | ||
43 | z = 0; | ||
44 | w = 0; | ||
45 | } | ||
46 | |||
47 | public float4(float _x, float _y, float _z, float _w) | ||
48 | { | ||
49 | x = _x; | ||
50 | y = _y; | ||
51 | z = _z; | ||
52 | w = _w; | ||
53 | } | ||
54 | |||
55 | public float4(float3 v, float _w) | ||
56 | { | ||
57 | x = v.x; | ||
58 | y = v.y; | ||
59 | z = v.z; | ||
60 | w = _w; | ||
61 | } | ||
62 | |||
63 | public float4(float4 f) | ||
64 | { | ||
65 | x = f.x; | ||
66 | y = f.y; | ||
67 | z = f.z; | ||
68 | w = f.w; | ||
69 | } | ||
70 | |||
71 | public float this[int i] | ||
72 | { | ||
73 | get | ||
74 | { | ||
75 | switch (i) | ||
76 | { | ||
77 | case 0: return x; | ||
78 | case 1: return y; | ||
79 | case 2: return z; | ||
80 | case 3: return w; | ||
81 | } | ||
82 | throw new ArgumentOutOfRangeException(); | ||
83 | } | ||
84 | } | ||
85 | |||
86 | public float3 xyz() | ||
87 | { | ||
88 | return new float3(x, y, z); | ||
89 | } | ||
90 | |||
91 | public void setxyz(float3 xyz) | ||
92 | { | ||
93 | x = xyz.x; | ||
94 | y = xyz.y; | ||
95 | z = xyz.z; | ||
96 | } | ||
97 | |||
98 | public override int GetHashCode() | ||
99 | { | ||
100 | return x.GetHashCode() ^ y.GetHashCode() ^ z.GetHashCode() ^ w.GetHashCode(); | ||
101 | } | ||
102 | |||
103 | public override bool Equals(object obj) | ||
104 | { | ||
105 | float4 f = obj as float4; | ||
106 | if (f == null) | ||
107 | return false; | ||
108 | |||
109 | return this == f; | ||
110 | } | ||
111 | |||
112 | public static float4 Homogenize(float3 v3) | ||
113 | { | ||
114 | return Homogenize(v3, 1.0f); | ||
115 | } | ||
116 | |||
117 | //C++ TO C# CONVERTER NOTE: C# does not allow default values for parameters. Overloaded methods are inserted above. | ||
118 | //ORIGINAL LINE: float4 Homogenize(const float3 &v3, const float &w =1.0f) | ||
119 | public static float4 Homogenize(float3 v3, float w) | ||
120 | { | ||
121 | return new float4(v3.x, v3.y, v3.z, w); | ||
122 | } | ||
123 | |||
124 | public static float4 cmul(float4 a, float4 b) | ||
125 | { | ||
126 | return new float4(a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w); | ||
127 | } | ||
128 | |||
129 | public static float4 operator +(float4 a, float4 b) | ||
130 | { | ||
131 | return new float4(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w); | ||
132 | } | ||
133 | public static float4 operator -(float4 a, float4 b) | ||
134 | { | ||
135 | return new float4(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w); | ||
136 | } | ||
137 | |||
138 | public static float4 operator *(float4 v, float4x4 m) | ||
139 | { | ||
140 | return v.x * m.x + v.y * m.y + v.z * m.z + v.w * m.w; // yes this actually works | ||
141 | } | ||
142 | |||
143 | public static bool operator ==(float4 a, float4 b) | ||
144 | { | ||
145 | // If both are null, or both are same instance, return true. | ||
146 | if (System.Object.ReferenceEquals(a, b)) | ||
147 | return true; | ||
148 | // If one is null, but not both, return false. | ||
149 | if (((object)a == null) || ((object)b == null)) | ||
150 | return false; | ||
151 | |||
152 | return (a.x == b.x && a.y == b.y && a.z == b.z && a.w == b.w); | ||
153 | } | ||
154 | |||
155 | public static bool operator !=(float4 a, float4 b) | ||
156 | { | ||
157 | return !(a == b); | ||
158 | } | ||
159 | |||
160 | public static float4 operator *(float4 v, float s) | ||
161 | { | ||
162 | return new float4(v.x * s, v.y * s, v.z * s, v.w * s); | ||
163 | } | ||
164 | |||
165 | public static float4 operator *(float s, float4 v) | ||
166 | { | ||
167 | return new float4(v.x * s, v.y * s, v.z * s, v.w * s); | ||
168 | } | ||
169 | } | ||
170 | } | ||
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/float4x4.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float4x4.cs new file mode 100644 index 0000000..7d1592f --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float4x4.cs | |||
@@ -0,0 +1,284 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | using System.Collections.Generic; | ||
30 | using System.Linq; | ||
31 | using System.Text; | ||
32 | |||
33 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
34 | { | ||
35 | public class float4x4 | ||
36 | { | ||
37 | public float4 x = new float4(); | ||
38 | public float4 y = new float4(); | ||
39 | public float4 z = new float4(); | ||
40 | public float4 w = new float4(); | ||
41 | |||
42 | public float4x4() | ||
43 | { | ||
44 | } | ||
45 | |||
46 | public float4x4(float4 _x, float4 _y, float4 _z, float4 _w) | ||
47 | { | ||
48 | x = new float4(_x); | ||
49 | y = new float4(_y); | ||
50 | z = new float4(_z); | ||
51 | w = new float4(_w); | ||
52 | } | ||
53 | |||
54 | public float4x4( | ||
55 | float m00, float m01, float m02, float m03, | ||
56 | float m10, float m11, float m12, float m13, | ||
57 | float m20, float m21, float m22, float m23, | ||
58 | float m30, float m31, float m32, float m33) | ||
59 | { | ||
60 | x = new float4(m00, m01, m02, m03); | ||
61 | y = new float4(m10, m11, m12, m13); | ||
62 | z = new float4(m20, m21, m22, m23); | ||
63 | w = new float4(m30, m31, m32, m33); | ||
64 | } | ||
65 | |||
66 | public float4x4(float4x4 m) | ||
67 | { | ||
68 | x = new float4(m.x); | ||
69 | y = new float4(m.y); | ||
70 | z = new float4(m.z); | ||
71 | w = new float4(m.w); | ||
72 | } | ||
73 | |||
74 | public float4 this[int i] | ||
75 | { | ||
76 | get | ||
77 | { | ||
78 | switch (i) | ||
79 | { | ||
80 | case 0: return x; | ||
81 | case 1: return y; | ||
82 | case 2: return z; | ||
83 | case 3: return w; | ||
84 | } | ||
85 | throw new ArgumentOutOfRangeException(); | ||
86 | } | ||
87 | set | ||
88 | { | ||
89 | switch (i) | ||
90 | { | ||
91 | case 0: x = value; return; | ||
92 | case 1: y = value; return; | ||
93 | case 2: z = value; return; | ||
94 | case 3: w = value; return; | ||
95 | } | ||
96 | throw new ArgumentOutOfRangeException(); | ||
97 | } | ||
98 | } | ||
99 | |||
100 | public override int GetHashCode() | ||
101 | { | ||
102 | return x.GetHashCode() ^ y.GetHashCode() ^ z.GetHashCode() ^ w.GetHashCode(); | ||
103 | } | ||
104 | |||
105 | public override bool Equals(object obj) | ||
106 | { | ||
107 | float4x4 m = obj as float4x4; | ||
108 | if (m == null) | ||
109 | return false; | ||
110 | |||
111 | return this == m; | ||
112 | } | ||
113 | |||
114 | public static float4x4 operator *(float4x4 a, float4x4 b) | ||
115 | { | ||
116 | return new float4x4(a.x * b, a.y * b, a.z * b, a.w * b); | ||
117 | } | ||
118 | |||
119 | public static bool operator ==(float4x4 a, float4x4 b) | ||
120 | { | ||
121 | return (a.x == b.x && a.y == b.y && a.z == b.z && a.w == b.w); | ||
122 | } | ||
123 | |||
124 | public static bool operator !=(float4x4 a, float4x4 b) | ||
125 | { | ||
126 | return !(a == b); | ||
127 | } | ||
128 | |||
129 | public static float4x4 Inverse(float4x4 m) | ||
130 | { | ||
131 | float4x4 d = new float4x4(); | ||
132 | //float dst = d.x.x; | ||
133 | float[] tmp = new float[12]; // temp array for pairs | ||
134 | float[] src = new float[16]; // array of transpose source matrix | ||
135 | float det; // determinant | ||
136 | // transpose matrix | ||
137 | for (int i = 0; i < 4; i++) | ||
138 | { | ||
139 | src[i] = m[i].x; | ||
140 | src[i + 4] = m[i].y; | ||
141 | src[i + 8] = m[i].z; | ||
142 | src[i + 12] = m[i].w; | ||
143 | } | ||
144 | // calculate pairs for first 8 elements (cofactors) | ||
145 | tmp[0] = src[10] * src[15]; | ||
146 | tmp[1] = src[11] * src[14]; | ||
147 | tmp[2] = src[9] * src[15]; | ||
148 | tmp[3] = src[11] * src[13]; | ||
149 | tmp[4] = src[9] * src[14]; | ||
150 | tmp[5] = src[10] * src[13]; | ||
151 | tmp[6] = src[8] * src[15]; | ||
152 | tmp[7] = src[11] * src[12]; | ||
153 | tmp[8] = src[8] * src[14]; | ||
154 | tmp[9] = src[10] * src[12]; | ||
155 | tmp[10] = src[8] * src[13]; | ||
156 | tmp[11] = src[9] * src[12]; | ||
157 | // calculate first 8 elements (cofactors) | ||
158 | d.x.x = tmp[0]*src[5] + tmp[3]*src[6] + tmp[4]*src[7]; | ||
159 | d.x.x -= tmp[1]*src[5] + tmp[2]*src[6] + tmp[5]*src[7]; | ||
160 | d.x.y = tmp[1]*src[4] + tmp[6]*src[6] + tmp[9]*src[7]; | ||
161 | d.x.y -= tmp[0]*src[4] + tmp[7]*src[6] + tmp[8]*src[7]; | ||
162 | d.x.z = tmp[2]*src[4] + tmp[7]*src[5] + tmp[10]*src[7]; | ||
163 | d.x.z -= tmp[3]*src[4] + tmp[6]*src[5] + tmp[11]*src[7]; | ||
164 | d.x.w = tmp[5]*src[4] + tmp[8]*src[5] + tmp[11]*src[6]; | ||
165 | d.x.w -= tmp[4]*src[4] + tmp[9]*src[5] + tmp[10]*src[6]; | ||
166 | d.y.x = tmp[1]*src[1] + tmp[2]*src[2] + tmp[5]*src[3]; | ||
167 | d.y.x -= tmp[0]*src[1] + tmp[3]*src[2] + tmp[4]*src[3]; | ||
168 | d.y.y = tmp[0]*src[0] + tmp[7]*src[2] + tmp[8]*src[3]; | ||
169 | d.y.y -= tmp[1]*src[0] + tmp[6]*src[2] + tmp[9]*src[3]; | ||
170 | d.y.z = tmp[3]*src[0] + tmp[6]*src[1] + tmp[11]*src[3]; | ||
171 | d.y.z -= tmp[2]*src[0] + tmp[7]*src[1] + tmp[10]*src[3]; | ||
172 | d.y.w = tmp[4]*src[0] + tmp[9]*src[1] + tmp[10]*src[2]; | ||
173 | d.y.w -= tmp[5]*src[0] + tmp[8]*src[1] + tmp[11]*src[2]; | ||
174 | // calculate pairs for second 8 elements (cofactors) | ||
175 | tmp[0] = src[2]*src[7]; | ||
176 | tmp[1] = src[3]*src[6]; | ||
177 | tmp[2] = src[1]*src[7]; | ||
178 | tmp[3] = src[3]*src[5]; | ||
179 | tmp[4] = src[1]*src[6]; | ||
180 | tmp[5] = src[2]*src[5]; | ||
181 | tmp[6] = src[0]*src[7]; | ||
182 | tmp[7] = src[3]*src[4]; | ||
183 | tmp[8] = src[0]*src[6]; | ||
184 | tmp[9] = src[2]*src[4]; | ||
185 | tmp[10] = src[0]*src[5]; | ||
186 | tmp[11] = src[1]*src[4]; | ||
187 | // calculate second 8 elements (cofactors) | ||
188 | d.z.x = tmp[0]*src[13] + tmp[3]*src[14] + tmp[4]*src[15]; | ||
189 | d.z.x -= tmp[1]*src[13] + tmp[2]*src[14] + tmp[5]*src[15]; | ||
190 | d.z.y = tmp[1]*src[12] + tmp[6]*src[14] + tmp[9]*src[15]; | ||
191 | d.z.y -= tmp[0]*src[12] + tmp[7]*src[14] + tmp[8]*src[15]; | ||
192 | d.z.z = tmp[2]*src[12] + tmp[7]*src[13] + tmp[10]*src[15]; | ||
193 | d.z.z -= tmp[3]*src[12] + tmp[6]*src[13] + tmp[11]*src[15]; | ||
194 | d.z.w = tmp[5]*src[12] + tmp[8]*src[13] + tmp[11]*src[14]; | ||
195 | d.z.w-= tmp[4]*src[12] + tmp[9]*src[13] + tmp[10]*src[14]; | ||
196 | d.w.x = tmp[2]*src[10] + tmp[5]*src[11] + tmp[1]*src[9]; | ||
197 | d.w.x-= tmp[4]*src[11] + tmp[0]*src[9] + tmp[3]*src[10]; | ||
198 | d.w.y = tmp[8]*src[11] + tmp[0]*src[8] + tmp[7]*src[10]; | ||
199 | d.w.y-= tmp[6]*src[10] + tmp[9]*src[11] + tmp[1]*src[8]; | ||
200 | d.w.z = tmp[6]*src[9] + tmp[11]*src[11] + tmp[3]*src[8]; | ||
201 | d.w.z-= tmp[10]*src[11] + tmp[2]*src[8] + tmp[7]*src[9]; | ||
202 | d.w.w = tmp[10]*src[10] + tmp[4]*src[8] + tmp[9]*src[9]; | ||
203 | d.w.w-= tmp[8]*src[9] + tmp[11]*src[10] + tmp[5]*src[8]; | ||
204 | // calculate determinant | ||
205 | det = src[0] * d.x.x + src[1] * d.x.y + src[2] * d.x.z + src[3] * d.x.w; | ||
206 | // calculate matrix inverse | ||
207 | det = 1/det; | ||
208 | for (int j = 0; j < 4; j++) | ||
209 | d[j] *= det; | ||
210 | return d; | ||
211 | } | ||
212 | |||
213 | public static float4x4 MatrixRigidInverse(float4x4 m) | ||
214 | { | ||
215 | float4x4 trans_inverse = MatrixTranslation(-m.w.xyz()); | ||
216 | float4x4 rot = new float4x4(m); | ||
217 | rot.w = new float4(0f, 0f, 0f, 1f); | ||
218 | return trans_inverse * MatrixTranspose(rot); | ||
219 | } | ||
220 | public static float4x4 MatrixTranspose(float4x4 m) | ||
221 | { | ||
222 | return new float4x4(m.x.x, m.y.x, m.z.x, m.w.x, m.x.y, m.y.y, m.z.y, m.w.y, m.x.z, m.y.z, m.z.z, m.w.z, m.x.w, m.y.w, m.z.w, m.w.w); | ||
223 | } | ||
224 | public static float4x4 MatrixPerspectiveFov(float fovy, float aspect, float zn, float zf) | ||
225 | { | ||
226 | float h = 1.0f / (float)Math.Tan(fovy / 2.0f); // view space height | ||
227 | float w = h / aspect; // view space width | ||
228 | return new float4x4(w, 0, 0, 0, 0, h, 0, 0, 0, 0, zf / (zn - zf), -1, 0, 0, zn * zf / (zn - zf), 0); | ||
229 | } | ||
230 | public static float4x4 MatrixTranslation(float3 t) | ||
231 | { | ||
232 | return new float4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, t.x, t.y, t.z, 1); | ||
233 | } | ||
234 | public static float4x4 MatrixRotationZ(float angle_radians) | ||
235 | { | ||
236 | float s = (float)Math.Sin(angle_radians); | ||
237 | float c = (float)Math.Cos(angle_radians); | ||
238 | return new float4x4(c, s, 0, 0, -s, c, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1); | ||
239 | } | ||
240 | public static float4x4 MatrixLookAt(float3 eye, float3 at, float3 up) | ||
241 | { | ||
242 | float4x4 m = new float4x4(); | ||
243 | m.w.w = 1.0f; | ||
244 | m.w.setxyz(eye); | ||
245 | m.z.setxyz(float3.normalize(eye - at)); | ||
246 | m.x.setxyz(float3.normalize(float3.cross(up, m.z.xyz()))); | ||
247 | m.y.setxyz(float3.cross(m.z.xyz(), m.x.xyz())); | ||
248 | return MatrixRigidInverse(m); | ||
249 | } | ||
250 | |||
251 | public static float4x4 MatrixFromQuatVec(Quaternion q, float3 v) | ||
252 | { | ||
253 | // builds a 4x4 transformation matrix based on orientation q and translation v | ||
254 | float qx2 = q.x * q.x; | ||
255 | float qy2 = q.y * q.y; | ||
256 | float qz2 = q.z * q.z; | ||
257 | |||
258 | float qxqy = q.x * q.y; | ||
259 | float qxqz = q.x * q.z; | ||
260 | float qxqw = q.x * q.w; | ||
261 | float qyqz = q.y * q.z; | ||
262 | float qyqw = q.y * q.w; | ||
263 | float qzqw = q.z * q.w; | ||
264 | |||
265 | return new float4x4( | ||
266 | 1 - 2 * (qy2 + qz2), | ||
267 | 2 * (qxqy + qzqw), | ||
268 | 2 * (qxqz - qyqw), | ||
269 | 0, | ||
270 | 2 * (qxqy - qzqw), | ||
271 | 1 - 2 * (qx2 + qz2), | ||
272 | 2 * (qyqz + qxqw), | ||
273 | 0, | ||
274 | 2 * (qxqz + qyqw), | ||
275 | 2 * (qyqz - qxqw), | ||
276 | 1 - 2 * (qx2 + qy2), | ||
277 | 0, | ||
278 | v.x, | ||
279 | v.y, | ||
280 | v.z, | ||
281 | 1.0f); | ||
282 | } | ||
283 | } | ||
284 | } | ||
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/int3.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/int3.cs new file mode 100644 index 0000000..9c5760d --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/int3.cs | |||
@@ -0,0 +1,128 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | |||
30 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
31 | { | ||
32 | public class int3 | ||
33 | { | ||
34 | public int x; | ||
35 | public int y; | ||
36 | public int z; | ||
37 | |||
38 | public int3() | ||
39 | { | ||
40 | } | ||
41 | |||
42 | public int3(int _x, int _y, int _z) | ||
43 | { | ||
44 | x = _x; | ||
45 | y = _y; | ||
46 | z = _z; | ||
47 | } | ||
48 | |||
49 | public int this[int i] | ||
50 | { | ||
51 | get | ||
52 | { | ||
53 | switch (i) | ||
54 | { | ||
55 | case 0: return x; | ||
56 | case 1: return y; | ||
57 | case 2: return z; | ||
58 | } | ||
59 | throw new ArgumentOutOfRangeException(); | ||
60 | } | ||
61 | set | ||
62 | { | ||
63 | switch (i) | ||
64 | { | ||
65 | case 0: x = value; return; | ||
66 | case 1: y = value; return; | ||
67 | case 2: z = value; return; | ||
68 | } | ||
69 | throw new ArgumentOutOfRangeException(); | ||
70 | } | ||
71 | } | ||
72 | |||
73 | public override int GetHashCode() | ||
74 | { | ||
75 | return x.GetHashCode() ^ y.GetHashCode() ^ z.GetHashCode(); | ||
76 | } | ||
77 | |||
78 | public override bool Equals(object obj) | ||
79 | { | ||
80 | int3 i = obj as int3; | ||
81 | if (i == null) | ||
82 | return false; | ||
83 | |||
84 | return this == i; | ||
85 | } | ||
86 | |||
87 | public static bool operator ==(int3 a, int3 b) | ||
88 | { | ||
89 | // If both are null, or both are same instance, return true. | ||
90 | if (System.Object.ReferenceEquals(a, b)) | ||
91 | return true; | ||
92 | // If one is null, but not both, return false. | ||
93 | if (((object)a == null) || ((object)b == null)) | ||
94 | return false; | ||
95 | |||
96 | for (int i = 0; i < 3; i++) | ||
97 | { | ||
98 | if (a[i] != b[i]) | ||
99 | return false; | ||
100 | } | ||
101 | return true; | ||
102 | } | ||
103 | |||
104 | public static bool operator !=(int3 a, int3 b) | ||
105 | { | ||
106 | return !(a == b); | ||
107 | } | ||
108 | |||
109 | public static int3 roll3(int3 a) | ||
110 | { | ||
111 | int tmp = a[0]; | ||
112 | a[0] = a[1]; | ||
113 | a[1] = a[2]; | ||
114 | a[2] = tmp; | ||
115 | return a; | ||
116 | } | ||
117 | |||
118 | public static bool isa(int3 a, int3 b) | ||
119 | { | ||
120 | return (a == b || roll3(a) == b || a == roll3(b)); | ||
121 | } | ||
122 | |||
123 | public static bool b2b(int3 a, int3 b) | ||
124 | { | ||
125 | return isa(a, new int3(b[2], b[1], b[0])); | ||
126 | } | ||
127 | } | ||
128 | } | ||
diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/int4.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/int4.cs new file mode 100644 index 0000000..c2b32e5 --- /dev/null +++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/int4.cs | |||
@@ -0,0 +1,66 @@ | |||
1 | /* The MIT License | ||
2 | * | ||
3 | * Copyright (c) 2010 Intel Corporation. | ||
4 | * All rights reserved. | ||
5 | * | ||
6 | * Based on the convexdecomposition library from | ||
7 | * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax. | ||
8 | * | ||
9 | * Permission is hereby granted, free of charge, to any person obtaining a copy | ||
10 | * of this software and associated documentation files (the "Software"), to deal | ||
11 | * in the Software without restriction, including without limitation the rights | ||
12 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell | ||
13 | * copies of the Software, and to permit persons to whom the Software is | ||
14 | * furnished to do so, subject to the following conditions: | ||
15 | * | ||
16 | * The above copyright notice and this permission notice shall be included in | ||
17 | * all copies or substantial portions of the Software. | ||
18 | * | ||
19 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR | ||
20 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, | ||
21 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE | ||
22 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER | ||
23 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, | ||
24 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN | ||
25 | * THE SOFTWARE. | ||
26 | */ | ||
27 | |||
28 | using System; | ||
29 | |||
30 | namespace OpenSim.Region.Physics.ConvexDecompositionDotNet | ||
31 | { | ||
32 | public class int4 | ||
33 | { | ||
34 | public int x; | ||
35 | public int y; | ||
36 | public int z; | ||
37 | public int w; | ||
38 | |||
39 | public int4() | ||
40 | { | ||
41 | } | ||
42 | |||
43 | public int4(int _x, int _y, int _z, int _w) | ||
44 | { | ||
45 | x = _x; | ||
46 | y = _y; | ||
47 | z = _z; | ||
48 | w = _w; | ||
49 | } | ||
50 | |||
51 | public int this[int i] | ||
52 | { | ||
53 | get | ||
54 | { | ||
55 | switch (i) | ||
56 | { | ||
57 | case 0: return x; | ||
58 | case 1: return y; | ||
59 | case 2: return z; | ||
60 | case 3: return w; | ||
61 | } | ||
62 | throw new ArgumentOutOfRangeException(); | ||
63 | } | ||
64 | } | ||
65 | } | ||
66 | } | ||