public virtual int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse)
{
return(0);
}
public void Sit(PhysicsActor actor, Vector3 avPos, Vector3 avCameraPosition, Vector3 offset, Vector3 avOffset, SitAvatarCallback PhysicsSitResponse)
{
if (!m_scene.haveActor(actor) || !(actor is OdePrim) || ((OdePrim)actor).prim_geom == IntPtr.Zero)
{
PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity);
return;
}
IntPtr geom = ((OdePrim)actor).prim_geom;
Vector3 geopos = SafeNativeMethods.GeomGetPositionOMV(geom);
Quaternion geomOri = SafeNativeMethods.GeomGetQuaternionOMV(geom);
// Vector3 geopos = actor.Position;
// Quaternion geomOri = actor.Orientation;
Quaternion geomInvOri = Quaternion.Conjugate(geomOri);
Quaternion ori = Quaternion.Identity;
Vector3 rayDir = geopos + offset - avCameraPosition;
float raylen = rayDir.Length();
if (raylen < 0.001f)
{
PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity);
return;
}
float t = 1 / raylen;
rayDir.X *= t;
rayDir.Y *= t;
rayDir.Z *= t;
raylen += 30f; // focal point may be far
List <ContactResult> rayResults;
rayResults = m_scene.RaycastActor(actor, avCameraPosition, rayDir, raylen, 1, RaySitFlags);
if (rayResults.Count == 0)
{
/* if this fundamental ray failed, then just fail so user can try another spot and not be sitted far on a big prim
* d.AABB aabb;
* d.GeomGetAABB(geom, out aabb);
* offset = new Vector3(avOffset.X, 0, aabb.MaxZ + avOffset.Z - geopos.Z);
* ori = geomInvOri;
* offset *= geomInvOri;
* PhysicsSitResponse(1, actor.LocalID, offset, ori);
*/
PhysicsSitResponse(0, actor.LocalID, offset, ori);
return;
}
int status = 1;
offset = rayResults[0].Pos - geopos;
SafeNativeMethods.GeomClassID geoclass = SafeNativeMethods.GeomGetClass(geom);
if (geoclass == SafeNativeMethods.GeomClassID.SphereClass)
{
float r = SafeNativeMethods.GeomSphereGetRadius(geom);
offset.Normalize();
offset *= r;
RotAroundZ(offset.X, offset.Y, ref ori);
if (r < 0.4f)
{
offset = new Vector3(0, 0, r);
}
else
{
if (offset.Z < 0.4f)
{
t = offset.Z;
float rsq = r * r;
t = 1.0f / (rsq - t * t);
offset.X *= t;
offset.Y *= t;
offset.Z = 0.4f;
t = rsq - 0.16f;
offset.X *= t;
offset.Y *= t;
}
else if (r > 0.8f && offset.Z > 0.8f * r)
{
status = 3;
avOffset.X = -avOffset.X;
avOffset.Z *= 1.6f;
}
}
offset += avOffset * ori;
ori = geomInvOri * ori;
offset *= geomInvOri;
PhysicsSitResponse(status, actor.LocalID, offset, ori);
return;
}
Vector3 norm = rayResults[0].Normal;
if (norm.Z < -0.4f)
{
PhysicsSitResponse(0, actor.LocalID, offset, Quaternion.Identity);
return;
}
float SitNormX = -rayDir.X;
float SitNormY = -rayDir.Y;
Vector3 pivot = geopos + offset;
float edgeNormalX = norm.X;
float edgeNormalY = norm.Y;
float edgeDirX = -rayDir.X;
float edgeDirY = -rayDir.Y;
Vector3 edgePos = rayResults[0].Pos;
float edgeDist = float.MaxValue;
bool foundEdge = false;
if (norm.Z < 0.5f)
{
float rayDist = 4.0f;
for (int i = 0; i < 6; i++)
{
pivot.X -= 0.01f * norm.X;
pivot.Y -= 0.01f * norm.Y;
pivot.Z -= 0.01f * norm.Z;
rayDir.X = -norm.X * norm.Z;
rayDir.Y = -norm.Y * norm.Z;
rayDir.Z = 1.0f - norm.Z * norm.Z;
rayDir.Normalize();
rayResults = m_scene.RaycastActor(actor, pivot, rayDir, rayDist, 1, RayFilterFlags.AllPrims);
if (rayResults.Count == 0)
{
break;
}
if (Math.Abs(rayResults[0].Normal.Z) < 0.7f)
{
rayDist -= rayResults[0].Depth;
if (rayDist < 0f)
{
break;
}
pivot = rayResults[0].Pos;
norm = rayResults[0].Normal;
edgeNormalX = norm.X;
edgeNormalY = norm.Y;
edgeDirX = -rayDir.X;
edgeDirY = -rayDir.Y;
}
else
{
foundEdge = true;
edgePos = rayResults[0].Pos;
break;
}
}
if (!foundEdge)
{
PhysicsSitResponse(0, actor.LocalID, offset, ori);
return;
}
avOffset.X *= 0.5f;
}
else if (norm.Z > 0.866f)
{
float toCamBaseX = avCameraPosition.X - pivot.X;
float toCamBaseY = avCameraPosition.Y - pivot.Y;
float toCamX = toCamBaseX;
float toCamY = toCamBaseY;
for (int j = 0; j < 4; j++)
{
float rayDist = 1.0f;
float curEdgeDist = 0.0f;
for (int i = 0; i < 3; i++)
{
pivot.Z -= 0.01f;
rayDir.X = toCamX;
rayDir.Y = toCamY;
rayDir.Z = (-toCamX * norm.X - toCamY * norm.Y) / norm.Z;
rayDir.Normalize();
rayResults = m_scene.RaycastActor(actor, pivot, rayDir, rayDist, 1, RayFilterFlags.AllPrims);
if (rayResults.Count == 0)
{
break;
}
curEdgeDist += rayResults[0].Depth;
if (rayResults[0].Normal.Z > 0.5f)
{
rayDist -= rayResults[0].Depth;
if (rayDist < 0f)
{
break;
}
pivot = rayResults[0].Pos;
norm = rayResults[0].Normal;
}
else
{
foundEdge = true;
if (curEdgeDist < edgeDist)
{
edgeDist = curEdgeDist;
edgeNormalX = rayResults[0].Normal.X;
edgeNormalY = rayResults[0].Normal.Y;
edgeDirX = rayDir.X;
edgeDirY = rayDir.Y;
edgePos = rayResults[0].Pos;
}
break;
}
}
if (foundEdge && edgeDist < 0.2f)
{
break;
}
pivot = geopos + offset;
switch (j)
{
case 0:
toCamX = -toCamBaseY;
toCamY = toCamBaseX;
break;
case 1:
toCamX = toCamBaseY;
toCamY = -toCamBaseX;
break;
case 2:
toCamX = -toCamBaseX;
toCamY = -toCamBaseY;
break;
default:
break;
}
}
if (!foundEdge)
{
avOffset.X = -avOffset.X;
avOffset.Z *= 1.6f;
RotAroundZ(SitNormX, SitNormY, ref ori);
offset += avOffset * ori;
ori = geomInvOri * ori;
offset *= geomInvOri;
PhysicsSitResponse(3, actor.LocalID, offset, ori);
return;
}
avOffset.X *= 0.5f;
}
SitNormX = edgeNormalX;
SitNormY = edgeNormalY;
if (edgeDirX * SitNormX + edgeDirY * SitNormY < 0)
{
SitNormX = -SitNormX;
SitNormY = -SitNormY;
}
RotAroundZ(SitNormX, SitNormY, ref ori);
offset = edgePos + avOffset * ori;
offset -= geopos;
ori = geomInvOri * ori;
offset *= geomInvOri;
PhysicsSitResponse(1, actor.LocalID, offset, ori);
return;
}
public override int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse)
{
Util.FireAndForget( delegate
{
ODESitAvatar sitAvatar = new ODESitAvatar(this, m_rayCastManager);
if(sitAvatar != null)
sitAvatar.Sit(actor, AbsolutePosition, CameraPosition, offset, AvatarSize, PhysicsSitResponse);
});
return 1;
}
public void Sit(PhysicsActor actor, Vector3 avPos, Vector3 avCameraPosition, Vector3 offset, Vector3 avOffset, SitAvatarCallback PhysicsSitResponse)
{
if (!m_scene.haveActor(actor) || !(actor is OdePrim) || ((OdePrim)actor).prim_geom == IntPtr.Zero)
{
PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity);
return;
}
IntPtr geom = ((OdePrim)actor).prim_geom;
Vector3 geopos = d.GeomGetPositionOMV(geom);
Quaternion geomOri = d.GeomGetQuaternionOMV(geom);
// Vector3 geopos = actor.Position;
// Quaternion geomOri = actor.Orientation;
Quaternion geomInvOri = Quaternion.Conjugate(geomOri);
Quaternion ori = Quaternion.Identity;
Vector3 rayDir = geopos + offset - avCameraPosition;
float raylen = rayDir.Length();
if (raylen < 0.001f)
{
PhysicsSitResponse(-1, actor.LocalID, offset, Quaternion.Identity);
return;
}
float t = 1 / raylen;
rayDir.X *= t;
rayDir.Y *= t;
rayDir.Z *= t;
raylen += 30f; // focal point may be far
List<ContactResult> rayResults;
rayResults = m_scene.RaycastActor(actor, avCameraPosition, rayDir, raylen, 1, RaySitFlags);
if (rayResults.Count == 0)
{
/* if this fundamental ray failed, then just fail so user can try another spot and not be sitted far on a big prim
d.AABB aabb;
d.GeomGetAABB(geom, out aabb);
offset = new Vector3(avOffset.X, 0, aabb.MaxZ + avOffset.Z - geopos.Z);
ori = geomInvOri;
offset *= geomInvOri;
PhysicsSitResponse(1, actor.LocalID, offset, ori);
*/
PhysicsSitResponse(0, actor.LocalID, offset, ori);
return;
}
int status = 1;
offset = rayResults[0].Pos - geopos;
d.GeomClassID geoclass = d.GeomGetClass(geom);
if (geoclass == d.GeomClassID.SphereClass)
{
float r = d.GeomSphereGetRadius(geom);
offset.Normalize();
offset *= r;
RotAroundZ(offset.X, offset.Y, ref ori);
if (r < 0.4f)
{
offset = new Vector3(0, 0, r);
}
else
{
if (offset.Z < 0.4f)
{
t = offset.Z;
float rsq = r * r;
t = 1.0f / (rsq - t * t);
offset.X *= t;
offset.Y *= t;
offset.Z = 0.4f;
t = rsq - 0.16f;
offset.X *= t;
offset.Y *= t;
}
else if (r > 0.8f && offset.Z > 0.8f * r)
{
status = 3;
avOffset.X = -avOffset.X;
avOffset.Z *= 1.6f;
}
}
offset += avOffset * ori;
ori = geomInvOri * ori;
offset *= geomInvOri;
PhysicsSitResponse(status, actor.LocalID, offset, ori);
return;
}
Vector3 norm = rayResults[0].Normal;
if (norm.Z < -0.4f)
{
PhysicsSitResponse(0, actor.LocalID, offset, Quaternion.Identity);
return;
}
float SitNormX = -rayDir.X;
float SitNormY = -rayDir.Y;
Vector3 pivot = geopos + offset;
float edgeNormalX = norm.X;
float edgeNormalY = norm.Y;
float edgeDirX = -rayDir.X;
float edgeDirY = -rayDir.Y;
Vector3 edgePos = rayResults[0].Pos;
float edgeDist = float.MaxValue;
bool foundEdge = false;
if (norm.Z < 0.5f)
{
float rayDist = 4.0f;
for (int i = 0; i < 6; i++)
{
pivot.X -= 0.01f * norm.X;
pivot.Y -= 0.01f * norm.Y;
pivot.Z -= 0.01f * norm.Z;
rayDir.X = -norm.X * norm.Z;
rayDir.Y = -norm.Y * norm.Z;
rayDir.Z = 1.0f - norm.Z * norm.Z;
rayDir.Normalize();
rayResults = m_scene.RaycastActor(actor, pivot, rayDir, rayDist, 1, RayFilterFlags.AllPrims);
if (rayResults.Count == 0)
break;
if (Math.Abs(rayResults[0].Normal.Z) < 0.7f)
{
rayDist -= rayResults[0].Depth;
if (rayDist < 0f)
break;
pivot = rayResults[0].Pos;
norm = rayResults[0].Normal;
edgeNormalX = norm.X;
edgeNormalY = norm.Y;
edgeDirX = -rayDir.X;
edgeDirY = -rayDir.Y;
}
else
{
foundEdge = true;
edgePos = rayResults[0].Pos;
break;
}
}
if (!foundEdge)
{
PhysicsSitResponse(0, actor.LocalID, offset, ori);
return;
}
avOffset.X *= 0.5f;
}
else if (norm.Z > 0.866f)
{
float toCamBaseX = avCameraPosition.X - pivot.X;
float toCamBaseY = avCameraPosition.Y - pivot.Y;
float toCamX = toCamBaseX;
float toCamY = toCamBaseY;
for (int j = 0; j < 4; j++)
{
float rayDist = 1.0f;
float curEdgeDist = 0.0f;
for (int i = 0; i < 3; i++)
{
pivot.Z -= 0.01f;
rayDir.X = toCamX;
rayDir.Y = toCamY;
rayDir.Z = (-toCamX * norm.X - toCamY * norm.Y) / norm.Z;
rayDir.Normalize();
rayResults = m_scene.RaycastActor(actor, pivot, rayDir, rayDist, 1, RayFilterFlags.AllPrims);
if (rayResults.Count == 0)
break;
curEdgeDist += rayResults[0].Depth;
if (rayResults[0].Normal.Z > 0.5f)
{
rayDist -= rayResults[0].Depth;
if (rayDist < 0f)
break;
pivot = rayResults[0].Pos;
norm = rayResults[0].Normal;
}
else
{
foundEdge = true;
if (curEdgeDist < edgeDist)
{
edgeDist = curEdgeDist;
edgeNormalX = rayResults[0].Normal.X;
edgeNormalY = rayResults[0].Normal.Y;
edgeDirX = rayDir.X;
edgeDirY = rayDir.Y;
edgePos = rayResults[0].Pos;
}
break;
}
}
if (foundEdge && edgeDist < 0.2f)
break;
pivot = geopos + offset;
switch (j)
{
case 0:
toCamX = -toCamBaseY;
toCamY = toCamBaseX;
break;
case 1:
toCamX = toCamBaseY;
toCamY = -toCamBaseX;
break;
case 2:
toCamX = -toCamBaseX;
toCamY = -toCamBaseY;
break;
default:
break;
}
}
if (!foundEdge)
{
avOffset.X = -avOffset.X;
avOffset.Z *= 1.6f;
RotAroundZ(SitNormX, SitNormY, ref ori);
offset += avOffset * ori;
ori = geomInvOri * ori;
offset *= geomInvOri;
PhysicsSitResponse(3, actor.LocalID, offset, ori);
return;
}
avOffset.X *= 0.5f;
}
SitNormX = edgeNormalX;
SitNormY = edgeNormalY;
if (edgeDirX * SitNormX + edgeDirY * SitNormY < 0)
{
SitNormX = -SitNormX;
SitNormY = -SitNormY;
}
RotAroundZ(SitNormX, SitNormY, ref ori);
offset = edgePos + avOffset * ori;
offset -= geopos;
ori = geomInvOri * ori;
offset *= geomInvOri;
PhysicsSitResponse(1, actor.LocalID, offset, ori);
return;
}
public virtual int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse)
{
return 0;
}