aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics/OdePlugin/ODECharacter.cs')
-rw-r--r--OpenSim/Region/Physics/OdePlugin/ODECharacter.cs477
1 files changed, 477 insertions, 0 deletions
diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
new file mode 100644
index 0000000..cda6af2
--- /dev/null
+++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
@@ -0,0 +1,477 @@
1using System;
2using System.Collections.Generic;
3using Axiom.Math;
4using Ode.NET;
5using OpenSim.Framework;
6using OpenSim.Region.Physics.Manager;
7
8namespace OpenSim.Region.Physics.OdePlugin
9{
10 public class OdeCharacter : PhysicsActor
11 {
12 private PhysicsVector _position;
13 private d.Vector3 _zeroPosition;
14 private bool _zeroFlag = false;
15 private bool m_lastUpdateSent = false;
16 private PhysicsVector _velocity;
17 private PhysicsVector _target_velocity;
18 private PhysicsVector _acceleration;
19 private PhysicsVector m_rotationalVelocity;
20 private static float PID_D = 3020.0f;
21 private static float PID_P = 7000.0f;
22 private static float POSTURE_SERVO = 10000.0f;
23 public static float CAPSULE_RADIUS = 0.5f;
24 public float CAPSULE_LENGTH = 0.79f;
25 private bool flying = false;
26 private bool m_iscolliding = false;
27 private bool m_iscollidingGround = false;
28 private bool m_wascolliding = false;
29 private bool m_wascollidingGround = false;
30 private bool m_alwaysRun = false;
31 private bool m_hackSentFall = false;
32 private bool m_hackSentFly = false;
33 private string m_name = "";
34
35 private bool[] m_colliderarr = new bool[11];
36 private bool[] m_colliderGroundarr = new bool[11];
37
38
39 private bool jumping = false;
40 //private float gravityAccel;
41 public IntPtr Body;
42 private OdeScene _parent_scene;
43 public IntPtr Shell;
44 public d.Mass ShellMass;
45 public bool collidelock = false;
46
47 public OdeCharacter(String avName, OdeScene parent_scene, PhysicsVector pos)
48 {
49 _velocity = new PhysicsVector();
50 _target_velocity = new PhysicsVector();
51 _position = pos;
52 _acceleration = new PhysicsVector();
53 _parent_scene = parent_scene;
54
55 for (int i = 0; i < 11; i++)
56 {
57 m_colliderarr[i] = false;
58 }
59
60 lock (OdeScene.OdeLock)
61 {
62
63 Shell = d.CreateCapsule(parent_scene.space, CAPSULE_RADIUS, CAPSULE_LENGTH);
64 d.MassSetCapsule(out ShellMass, 50.0f, 3, 0.4f, 1.0f);
65 Body = d.BodyCreate(parent_scene.world);
66 d.BodySetMass(Body, ref ShellMass);
67 d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
68 d.GeomSetBody(Shell, Body);
69 }
70 m_name = avName;
71 parent_scene.geom_name_map[Shell] = avName;
72 parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
73 }
74 public override int PhysicsActorType
75 {
76 get { return (int)ActorTypes.Agent; }
77 set { return; }
78 }
79 public override bool SetAlwaysRun
80 {
81 get { return m_alwaysRun; }
82 set { m_alwaysRun = value; }
83 }
84 public override bool IsPhysical
85 {
86 get { return false; }
87 set { return; }
88 }
89 public override bool ThrottleUpdates
90 {
91 get { return false; }
92 set { return; }
93 }
94 public override bool Flying
95 {
96 get { return flying; }
97 set { flying = value; }
98 }
99 public override bool IsColliding
100 {
101
102 get { return m_iscolliding; }
103 set
104 {
105 int i;
106 int truecount = 0;
107 int falsecount = 0;
108
109 if (m_colliderarr.Length >= 10)
110 {
111 for (i = 0; i < 10; i++)
112 {
113 m_colliderarr[i] = m_colliderarr[i + 1];
114 }
115 }
116 m_colliderarr[10] = value;
117
118 for (i = 0; i < 11; i++)
119 {
120 if (m_colliderarr[i])
121 {
122 truecount++;
123 }
124 else
125 {
126 falsecount++;
127 }
128 }
129
130 // Equal truecounts and false counts means we're colliding with something.
131
132 if (falsecount > 1.2 * truecount)
133 {
134 m_iscolliding = false;
135 }
136 else
137 {
138 m_iscolliding = true;
139 }
140 if (m_wascolliding != m_iscolliding)
141 {
142 base.SendCollisionUpdate(new CollisionEventUpdate());
143 }
144 m_wascolliding = m_iscolliding;
145 }
146 }
147 public override bool CollidingGround
148 {
149 get { return m_iscollidingGround; }
150 set
151 {
152 int i;
153 int truecount = 0;
154 int falsecount = 0;
155
156 if (m_colliderGroundarr.Length >= 10)
157 {
158 for (i = 0; i < 10; i++)
159 {
160 m_colliderGroundarr[i] = m_colliderGroundarr[i + 1];
161 }
162 }
163 m_colliderGroundarr[10] = value;
164
165 for (i = 0; i < 11; i++)
166 {
167 if (m_colliderGroundarr[i])
168 {
169 truecount++;
170 }
171 else
172 {
173 falsecount++;
174 }
175 }
176
177 // Equal truecounts and false counts means we're colliding with something.
178
179 if (falsecount > 1.2 * truecount)
180 {
181 m_iscollidingGround = false;
182 }
183 else
184 {
185 m_iscollidingGround = true;
186 }
187 if (m_wascollidingGround != m_iscollidingGround)
188 {
189 //base.SendCollisionUpdate(new CollisionEventUpdate());
190 }
191 m_wascollidingGround = m_iscollidingGround;
192 }
193 }
194 public override bool CollidingObj
195 {
196 get { return false; }
197 set { return; }
198 }
199
200 public override PhysicsVector Position
201 {
202 get { return _position; }
203 set
204 {
205 lock (OdeScene.OdeLock)
206 {
207 d.BodySetPosition(Body, value.X, value.Y, value.Z);
208 _position = value;
209 }
210 }
211 }
212 public override PhysicsVector RotationalVelocity
213 {
214 get { return m_rotationalVelocity; }
215 set { m_rotationalVelocity = value; }
216 }
217 public override PhysicsVector Size
218 {
219 get { return new PhysicsVector(CAPSULE_RADIUS * 2, CAPSULE_RADIUS * 2, CAPSULE_LENGTH); }
220 set
221 {
222 lock (OdeScene.OdeLock)
223 {
224 PhysicsVector SetSize = value;
225 float prevCapsule = CAPSULE_LENGTH;
226 float capsuleradius = CAPSULE_RADIUS;
227 capsuleradius = 0.2f;
228
229 CAPSULE_LENGTH = (SetSize.Z - ((SetSize.Z * 0.43f))); // subtract 43% of the size
230 d.BodyDestroy(Body);
231 d.GeomDestroy(Shell);
232 //OpenSim.Framework.Console.MainLog.Instance.Verbose("PHYSICS", "Set Avatar Height To: " + (CAPSULE_RADIUS + CAPSULE_LENGTH));
233 Shell = d.CreateCapsule(_parent_scene.space, capsuleradius, CAPSULE_LENGTH);
234 d.MassSetCapsule(out ShellMass, 50.0f, 3, CAPSULE_RADIUS, CAPSULE_LENGTH);
235 Body = d.BodyCreate(_parent_scene.world);
236 d.BodySetMass(Body, ref ShellMass);
237 d.BodySetPosition(Body, _position.X, _position.Y, _position.Z + Math.Abs(CAPSULE_LENGTH - prevCapsule));
238 d.GeomSetBody(Shell, Body);
239 }
240 _parent_scene.geom_name_map[Shell] = m_name;
241 _parent_scene.actor_name_map[Shell] = (PhysicsActor)this;
242 }
243 }
244
245 public override PrimitiveBaseShape Shape
246 {
247 set
248 {
249 return;
250 }
251 }
252
253 public override PhysicsVector Velocity
254 {
255 get { return _velocity; }
256 set { _target_velocity = value; }
257 }
258
259 public override bool Kinematic
260 {
261 get { return false; }
262 set { }
263 }
264
265 public override Quaternion Orientation
266 {
267 get { return Quaternion.Identity; }
268 set { }
269 }
270
271 public override PhysicsVector Acceleration
272 {
273 get { return _acceleration; }
274 }
275
276 public void SetAcceleration(PhysicsVector accel)
277 {
278 _acceleration = accel;
279 }
280
281 public override void AddForce(PhysicsVector force)
282 {
283
284 _target_velocity.X += force.X;
285 _target_velocity.Y += force.Y;
286 _target_velocity.Z += force.Z;
287
288 //m_lastUpdateSent = false;
289 }
290 public void doForce(PhysicsVector force)
291 {
292 if (!collidelock)
293 {
294 d.BodyAddForce(Body, force.X, force.Y, force.Z);
295
296 // ok -- let's stand up straight!
297 d.Vector3 feet;
298 d.Vector3 head;
299 d.BodyGetRelPointPos(Body, 0.0f, 0.0f, -1.0f, out feet);
300 d.BodyGetRelPointPos(Body, 0.0f, 0.0f, 1.0f, out head);
301 float posture = head.Z - feet.Z;
302
303 // restoring force proportional to lack of posture:
304 float servo = (2.5f - posture) * POSTURE_SERVO;
305 d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, servo, 0.0f, 0.0f, 1.0f);
306 d.BodyAddForceAtRelPos(Body, 0.0f, 0.0f, -servo, 0.0f, 0.0f, -1.0f);
307 //m_lastUpdateSent = false;
308
309 }
310
311 }
312 public override void SetMomentum(PhysicsVector momentum)
313 {
314
315 }
316
317 public void Move(float timeStep)
318 {
319 // no lock; for now it's only called from within Simulate()
320 PhysicsVector vec = new PhysicsVector();
321 d.Vector3 vel = d.BodyGetLinearVel(Body);
322 float movementdivisor = 1f;
323
324 if (!m_alwaysRun)
325 {
326 movementdivisor = 1.3f;
327 }
328 else
329 {
330 movementdivisor = 0.8f;
331
332 }
333
334 // if velocity is zero, use position control; otherwise, velocity control
335 if (_target_velocity.X == 0.0f && _target_velocity.Y == 0.0f && _target_velocity.Z == 0.0f && m_iscolliding)
336 {
337 // keep track of where we stopped. No more slippin' & slidin'
338 if (!_zeroFlag)
339 {
340 _zeroFlag = true;
341 _zeroPosition = d.BodyGetPosition(Body);
342 }
343 d.Vector3 pos = d.BodyGetPosition(Body);
344 vec.X = (_target_velocity.X - vel.X) * PID_D + (_zeroPosition.X - pos.X) * PID_P;
345 vec.Y = (_target_velocity.Y - vel.Y) * PID_D + (_zeroPosition.Y - pos.Y) * PID_P;
346 if (flying)
347 {
348 vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
349 }
350 }
351 else
352 {
353
354 _zeroFlag = false;
355 if (m_iscolliding || flying)
356 {
357
358 vec.X = ((_target_velocity.X / movementdivisor) - vel.X) * PID_D;
359 vec.Y = ((_target_velocity.Y / movementdivisor) - vel.Y) * PID_D;
360 }
361 if (m_iscolliding && !flying && _target_velocity.Z > 0.0f)
362 {
363 d.Vector3 pos = d.BodyGetPosition(Body);
364 vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
365 if (_target_velocity.X > 0)
366 {
367 vec.X = ((_target_velocity.X - vel.X) / 1.2f) * PID_D;
368 }
369 if (_target_velocity.Y > 0)
370 {
371 vec.Y = ((_target_velocity.Y - vel.Y) / 1.2f) * PID_D;
372 }
373 }
374 else if (!m_iscolliding && !flying)
375 {
376 d.Vector3 pos = d.BodyGetPosition(Body);
377 if (_target_velocity.X > 0)
378 {
379 vec.X = ((_target_velocity.X - vel.X) / 1.2f) * PID_D;
380 }
381 if (_target_velocity.Y > 0)
382 {
383 vec.Y = ((_target_velocity.Y - vel.Y) / 1.2f) * PID_D;
384 }
385
386 }
387
388
389 if (flying)
390 {
391 vec.Z = (_target_velocity.Z - vel.Z) * PID_D;
392 }
393 }
394 if (flying)
395 {
396 vec.Z += 10.0f;
397 }
398
399
400 doForce(vec);
401 }
402
403 public void UpdatePositionAndVelocity()
404 {
405 // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
406 d.Vector3 vec = d.BodyGetPosition(Body);
407
408 // kluge to keep things in bounds. ODE lets dead avatars drift away (they should be removed!)
409 if (vec.X < 0.0f) vec.X = 0.0f;
410 if (vec.Y < 0.0f) vec.Y = 0.0f;
411 if (vec.X > 255.95f) vec.X = 255.95f;
412 if (vec.Y > 255.95f) vec.Y = 255.95f;
413
414 _position.X = vec.X;
415 _position.Y = vec.Y;
416 _position.Z = vec.Z;
417
418 if (_zeroFlag)
419 {
420 _velocity.X = 0.0f;
421 _velocity.Y = 0.0f;
422 _velocity.Z = 0.0f;
423 if (!m_lastUpdateSent)
424 {
425 m_lastUpdateSent = true;
426 base.RequestPhysicsterseUpdate();
427 string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
428 int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
429 if (primScenAvatarIn == "0")
430 {
431 OpenSim.Framework.Console.MainLog.Instance.Verbose("Physics", "Avatar " + m_name + " in space with no prim. Arr:':" + arrayitem[0].ToString() + "," + arrayitem[1].ToString());
432 }
433 else
434 {
435 OpenSim.Framework.Console.MainLog.Instance.Verbose("Physics", "Avatar " + m_name + " in Prim space':" + primScenAvatarIn + ". Arr:" + arrayitem[0].ToString() + "," + arrayitem[1].ToString());
436 }
437
438 }
439 }
440 else
441 {
442 m_lastUpdateSent = false;
443 vec = d.BodyGetLinearVel(Body);
444 _velocity.X = (vec.X);
445 _velocity.Y = (vec.Y);
446
447 _velocity.Z = (vec.Z);
448 if (_velocity.Z < -6 && !m_hackSentFall)
449 {
450 m_hackSentFall = true;
451 base.SendCollisionUpdate(new CollisionEventUpdate());
452 }
453 else if (flying && !m_hackSentFly)
454 {
455 //m_hackSentFly = true;
456 //base.SendCollisionUpdate(new CollisionEventUpdate());
457 }
458 else
459 {
460 m_hackSentFly = false;
461 m_hackSentFall = false;
462 }
463 }
464 }
465
466 public void Destroy()
467 {
468 lock (OdeScene.OdeLock)
469 {
470 d.GeomDestroy(Shell);
471 _parent_scene.geom_name_map.Remove(Shell);
472 d.BodyDestroy(Body);
473 }
474 }
475 }
476
477}