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