aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs')
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs356
1 files changed, 356 insertions, 0 deletions
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs
new file mode 100644
index 0000000..e9023c3
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODESitAvatar.cs
@@ -0,0 +1,356 @@
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// Ubit 2012
28using System;
29using System.Collections.Generic;
30using System.Reflection;
31using System.Runtime.InteropServices;
32using System.Text;
33using OpenSim.Framework;
34using OpenSim.Region.Physics.Manager;
35using OdeAPI;
36using log4net;
37using OpenMetaverse;
38
39namespace OpenSim.Region.Physics.OdePlugin
40{
41 /// <summary>
42 /// </summary>
43 public class ODESitAvatar
44 {
45 private OdeScene m_scene;
46 private ODERayCastRequestManager m_raymanager;
47
48 public ODESitAvatar(OdeScene pScene, ODERayCastRequestManager raymanager)
49 {
50 m_scene = pScene;
51 m_raymanager = raymanager;
52 }
53
54 private static Vector3 SitAjust = new Vector3(0, 0, 0.4f);
55 private const RayFilterFlags RaySitFlags = RayFilterFlags.AllPrims | RayFilterFlags.ClosestHit;
56
57 private void RotAroundZ(float x, float y, ref Quaternion ori)
58 {
59 double ang = Math.Atan2(y, x);
60 ang *= 0.5d;
61 float s = (float)Math.Sin(ang);
62 float c = (float)Math.Cos(ang);
63
64 ori.X = 0;
65 ori.Y = 0;
66 ori.Z = s;
67 ori.W = c;
68 }
69
70
71 public void Sit(PhysicsActor actor, Vector3 avPos, Vector3 avCameraPosition, Vector3 offset, Vector3 avOffset, SitAvatarCallback PhysicsSitResponse)
72 {
73 if (!m_scene.haveActor(actor) || !(actor is OdePrim) || ((OdePrim)actor).prim_geom == IntPtr.Zero)
74 {
75 PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity);
76 return;
77 }
78
79 IntPtr geom = ((OdePrim)actor).prim_geom;
80
81// Vector3 geopos = d.GeomGetPositionOMV(geom);
82// Quaternion geomOri = d.GeomGetQuaternionOMV(geom);
83
84 Vector3 geopos = actor.Position;
85 Quaternion geomOri = actor.Orientation;
86
87 Quaternion geomInvOri = Quaternion.Conjugate(geomOri);
88
89 Quaternion ori = Quaternion.Identity;
90
91 Vector3 rayDir = geopos + offset - avCameraPosition;
92
93 float raylen = rayDir.Length();
94 if (raylen < 0.001f)
95 {
96 PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity);
97 return;
98 }
99 float t = 1 / raylen;
100 rayDir.X *= t;
101 rayDir.Y *= t;
102 rayDir.Z *= t;
103
104 raylen += 30f; // focal point may be far
105 List<ContactResult> rayResults;
106
107 rayResults = m_scene.RaycastActor(actor, avCameraPosition, rayDir, raylen, 1, RaySitFlags);
108 if (rayResults.Count == 0)
109 {
110/* if this fundamental ray failed, then just fail so user can try another spot and not be sitted far on a big prim
111 d.AABB aabb;
112 d.GeomGetAABB(geom, out aabb);
113 offset = new Vector3(avOffset.X, 0, aabb.MaxZ + avOffset.Z - geopos.Z);
114 ori = geomInvOri;
115 offset *= geomInvOri;
116 PhysicsSitResponse(1, actor.LocalID, offset, ori);
117*/
118 PhysicsSitResponse(0, actor.LocalID, offset, ori);
119 return;
120 }
121
122 int status = 1;
123
124 offset = rayResults[0].Pos - geopos;
125
126 d.GeomClassID geoclass = d.GeomGetClass(geom);
127
128 if (geoclass == d.GeomClassID.SphereClass)
129 {
130 float r = d.GeomSphereGetRadius(geom);
131
132 offset.Normalize();
133 offset *= r;
134
135 RotAroundZ(offset.X, offset.Y, ref ori);
136
137 if (r < 0.4f)
138 {
139 offset = new Vector3(0, 0, r);
140 }
141 else
142 {
143 if (offset.Z < 0.4f)
144 {
145 t = offset.Z;
146 float rsq = r * r;
147
148 t = 1.0f / (rsq - t * t);
149 offset.X *= t;
150 offset.Y *= t;
151 offset.Z = 0.4f;
152 t = rsq - 0.16f;
153 offset.X *= t;
154 offset.Y *= t;
155 }
156 else if (r > 0.8f && offset.Z > 0.8f * r)
157 {
158 status = 3;
159 avOffset.X = -avOffset.X;
160 avOffset.Z *= 1.6f;
161 }
162 }
163
164 offset += avOffset * ori;
165
166 ori = geomInvOri * ori;
167 offset *= geomInvOri;
168
169 PhysicsSitResponse(status, actor.LocalID, offset, ori);
170 return;
171 }
172
173 Vector3 norm = rayResults[0].Normal;
174
175 if (norm.Z < -0.4f)
176 {
177 PhysicsSitResponse(0, actor.LocalID, offset, Quaternion.Identity);
178 return;
179 }
180
181
182 float SitNormX = -rayDir.X;
183 float SitNormY = -rayDir.Y;
184
185 Vector3 pivot = geopos + offset;
186
187 float edgeNormalX = norm.X;
188 float edgeNormalY = norm.Y;
189 float edgeDirX = -rayDir.X;
190 float edgeDirY = -rayDir.Y;
191 Vector3 edgePos = rayResults[0].Pos;
192 float edgeDist = float.MaxValue;
193
194 bool foundEdge = false;
195
196 if (norm.Z < 0.5f)
197 {
198 float rayDist = 4.0f;
199
200 for (int i = 0; i < 6; i++)
201 {
202 pivot.X -= 0.01f * norm.X;
203 pivot.Y -= 0.01f * norm.Y;
204 pivot.Z -= 0.01f * norm.Z;
205
206 rayDir.X = -norm.X * norm.Z;
207 rayDir.Y = -norm.Y * norm.Z;
208 rayDir.Z = 1.0f - norm.Z * norm.Z;
209 rayDir.Normalize();
210
211 rayResults = m_scene.RaycastActor(actor, pivot, rayDir, rayDist, 1, RayFilterFlags.AllPrims);
212 if (rayResults.Count == 0)
213 break;
214
215 if (Math.Abs(rayResults[0].Normal.Z) < 0.7f)
216 {
217 rayDist -= rayResults[0].Depth;
218 if (rayDist < 0f)
219 break;
220
221 pivot = rayResults[0].Pos;
222 norm = rayResults[0].Normal;
223 edgeNormalX = norm.X;
224 edgeNormalY = norm.Y;
225 edgeDirX = -rayDir.X;
226 edgeDirY = -rayDir.Y;
227 }
228 else
229 {
230 foundEdge = true;
231 edgePos = rayResults[0].Pos;
232 break;
233 }
234 }
235
236 if (!foundEdge)
237 {
238 PhysicsSitResponse(0, actor.LocalID, offset, ori);
239 return;
240 }
241 avOffset.X *= 0.5f;
242 }
243
244 else if (norm.Z > 0.866f)
245 {
246 float toCamBaseX = avCameraPosition.X - pivot.X;
247 float toCamBaseY = avCameraPosition.Y - pivot.Y;
248 float toCamX = toCamBaseX;
249 float toCamY = toCamBaseY;
250
251 for (int j = 0; j < 4; j++)
252 {
253 float rayDist = 1.0f;
254 float curEdgeDist = 0.0f;
255
256 for (int i = 0; i < 3; i++)
257 {
258 pivot.Z -= 0.01f;
259 rayDir.X = toCamX;
260 rayDir.Y = toCamY;
261 rayDir.Z = (-toCamX * norm.X - toCamY * norm.Y) / norm.Z;
262 rayDir.Normalize();
263
264 rayResults = m_scene.RaycastActor(actor, pivot, rayDir, rayDist, 1, RayFilterFlags.AllPrims);
265 if (rayResults.Count == 0)
266 break;
267
268 curEdgeDist += rayResults[0].Depth;
269
270 if (rayResults[0].Normal.Z > 0.5f)
271 {
272 rayDist -= rayResults[0].Depth;
273 if (rayDist < 0f)
274 break;
275
276 pivot = rayResults[0].Pos;
277 norm = rayResults[0].Normal;
278 }
279 else
280 {
281 foundEdge = true;
282 if (curEdgeDist < edgeDist)
283 {
284 edgeDist = curEdgeDist;
285 edgeNormalX = rayResults[0].Normal.X;
286 edgeNormalY = rayResults[0].Normal.Y;
287 edgeDirX = rayDir.X;
288 edgeDirY = rayDir.Y;
289 edgePos = rayResults[0].Pos;
290 }
291 break;
292 }
293 }
294 if (foundEdge && edgeDist < 0.2f)
295 break;
296
297 pivot = geopos + offset;
298
299 switch (j)
300 {
301 case 0:
302 toCamX = -toCamBaseY;
303 toCamY = toCamBaseX;
304 break;
305 case 1:
306 toCamX = toCamBaseY;
307 toCamY = -toCamBaseX;
308 break;
309 case 2:
310 toCamX = -toCamBaseX;
311 toCamY = -toCamBaseY;
312 break;
313 default:
314 break;
315 }
316 }
317
318 if (!foundEdge)
319 {
320 avOffset.X = -avOffset.X;
321 avOffset.Z *= 1.6f;
322
323 RotAroundZ(SitNormX, SitNormY, ref ori);
324
325 offset += avOffset * ori;
326
327 ori = geomInvOri * ori;
328 offset *= geomInvOri;
329
330 PhysicsSitResponse(3, actor.LocalID, offset, ori);
331 return;
332 }
333 avOffset.X *= 0.5f;
334 }
335
336 SitNormX = edgeNormalX;
337 SitNormY = edgeNormalY;
338 if (edgeDirX * SitNormX + edgeDirY * SitNormY < 0)
339 {
340 SitNormX = -SitNormX;
341 SitNormY = -SitNormY;
342 }
343
344 RotAroundZ(SitNormX, SitNormY, ref ori);
345
346 offset = edgePos + avOffset * ori;
347 offset -= geopos;
348
349 ori = geomInvOri * ori;
350 offset *= geomInvOri;
351
352 PhysicsSitResponse(1, actor.LocalID, offset, ori);
353 return;
354 }
355 }
356} \ No newline at end of file