Esempio n. 1
0
        // 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;
            }
        }
Esempio n. 2
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  = 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;
        }