// Draw an object in the scene
static void drawGeom(IntPtr geom)
{
IntPtr body = d.GeomGetBody(geom);
d.Vector3 pos;
d.BodyCopyPosition(body, out pos);
d.Matrix3 R;
d.BodyCopyRotation(body, out R);
d.GeomClassID type = d.GeomGetClass(geom);
switch (type)
{
case d.GeomClassID.BoxClass:
d.Vector3 sides;
d.GeomBoxGetLengths(geom, out sides);
ds.DrawBox(ref pos, ref R, ref sides);
break;
case d.GeomClassID.CapsuleClass:
dReal radius, length;
d.GeomCapsuleGetParams(geom, out radius, out length);
ds.DrawCapsule(ref pos, ref R, length, radius);
break;
case d.GeomClassID.ConvexClass:
ds.DrawConvex(ref pos, ref R, planes, planes.Length / 4, points, points.Length / 3, polygons);
break;
}
}
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;
}