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