コード例 #1
0
ファイル: Box.cs プロジェクト: willnode/Qu3e-Sharp
        public void Render(Transform tx, bool awake, Render render)
        {
            Transform world = Transform.Mul(tx, local);

            Vec3[] vertices = new Vec3[8] {
                new Vec3(-e.x, -e.y, -e.z),
                new  Vec3(-e.x, -e.y, e.z),
                new  Vec3(-e.x, e.y, -e.z),
                new   Vec3(-e.x, e.y, e.z),
                new   Vec3(e.x, -e.y, -e.z),
                new         Vec3(e.x, -e.y, e.z),
                new     Vec3(e.x, e.y, -e.z),
                new Vec3(e.x, e.y, e.z)
            };

            for (int i = 0; i < 36; i += 3)
            {
                Vec3 a = Transform.Mul(world, vertices[kBoxIndices[i]]);
                Vec3 b = Transform.Mul(world, vertices[kBoxIndices[i + 1]]);
                Vec3 c = Transform.Mul(world, vertices[kBoxIndices[i + 2]]);

                Vec3 n = Vec3.Normalize(Vec3.Cross(b - a, c - a));

                //render->SetPenColor( 0.2f, 0.4f, 0.7f, 0.5f );
                //render->SetPenPosition( a.x, a.y, a.z );
                //render->Line( b.x, b.y, b.z );
                //render->Line( c.x, c.y, c.z );
                //render->Line( a.x, a.y, a.z );

                render.SetTriNormal(n.x, n.y, n.z);
                render.Triangle(a.x, a.y, a.z, b.x, b.y, b.z, c.x, c.y, c.z);
            }
        }
コード例 #2
0
ファイル: Box.cs プロジェクト: willnode/Qu3e-Sharp
        public bool Raycast(Transform tx, RaycastData raycast)
        {
            Transform world   = Transform.Mul(tx, local);
            Vec3      d       = Transform.MulT(world.rotation, raycast.dir);
            Vec3      p       = Transform.MulT(world, raycast.start);
            double    epsilon = 1e-8;
            double    tmin    = 0;
            double    tmax    = raycast.t;

            // t = (e[ i ] - p.[ i ]) / d[ i ]
            double t0;
            double t1;
            Vec3   n0 = new Vec3();

            for (int i = 0; i < 3; ++i)
            {
                // Check for ray parallel to and outside of AABB
                if (Math.Abs(d[i]) < epsilon)
                {
                    // Detect separating axes
                    if (p[i] < -e[i] || p[i] > e[i])
                    {
                        return(false);
                    }
                }

                else
                {
                    double d0 = 1 / d[i];
                    double s  = Math.Sign(d[i]);
                    double ei = e[i] * s;
                    Vec3   n  = new Vec3(0, 0, 0);
                    n[i] = -s;

                    t0 = -(ei + p[i]) * d0;
                    t1 = (ei - p[i]) * d0;

                    if (t0 > tmin)
                    {
                        n0   = n;
                        tmin = t0;
                    }

                    tmax = Math.Min(tmax, t1);

                    if (tmin > tmax)
                    {
                        return(false);
                    }
                }
            }

            raycast.normal = Transform.Mul(world.rotation, n0);
            raycast.toi    = tmin;

            return(true);
        }
コード例 #3
0
        //--------------------------------------------------------------------------------------------------
        public static void SupportEdge(Transform tx, Vec3 e, Vec3 n, out Vec3 aresult, out Vec3 bresult)
        {
            n = Transform.MulT(tx.rotation, n);
            Vec3 absN = Vec3.Abs(n);
            Vec3 a = new Vec3(), b = new Vec3();

            // x > y
            if (absN.x > absN.y)
            {
                // x > y > z
                if (absN.y > absN.z)
                {
                    a.Set(e.x, e.y, e.z);
                    b.Set(e.x, e.y, -e.z);
                }

                // x > z > y || z > x > y
                else
                {
                    a.Set(e.x, e.y, e.z);
                    b.Set(e.x, -e.y, e.z);
                }
            }

            // y > x
            else
            {
                // y > x > z
                if (absN.x > absN.z)
                {
                    a.Set(e.x, e.y, e.z);
                    b.Set(e.x, e.y, -e.z);
                }

                // z > y > x || y > z > x
                else
                {
                    a.Set(e.x, e.y, e.z);
                    b.Set(-e.x, e.y, e.z);
                }
            }

            double signx = Sign(n.x);
            double signy = Sign(n.y);
            double signz = Sign(n.z);

            a.x *= signx;
            a.y *= signy;
            a.z *= signz;
            b.x *= signx;
            b.y *= signy;
            b.z *= signz;

            aresult = Transform.Mul(tx, a);
            bresult = Transform.Mul(tx, b);
        }
コード例 #4
0
        //--------------------------------------------------------------------------------------------------
        // Resources (also see q3BoxtoBox's resources):
        // http://www.randygaul.net/2013/10/27/sutherland-hodgman-clipping/
        public static int Clip(Vec3 rPos, Vec3 e, byte[] clipEdges, Mat3 basis, ClipVertex[] incident, ClipVertex[] resultVerts, double[] resultDepths)
        {
            int inCount = 4;
            int resultCount;

            for (int i = 0; i < 4; ++i)
            {
                input[i].v = Transform.MulT(basis, incident[i].v - rPos);
            }

            resultCount = Orthographic(1, e.x, 0, clipEdges[0], input, inCount, result);

            if (resultCount == 0)
            {
                return(0);
            }

            inCount = Orthographic(1, e.y, 1, clipEdges[1], result, resultCount, input);

            if (inCount == 0)
            {
                return(0);
            }

            resultCount = Orthographic(-1, e.x, 0, clipEdges[2], input, inCount, result);

            if (resultCount == 0)
            {
                return(0);
            }

            inCount = Orthographic(-1, e.y, 1, clipEdges[3], result, resultCount, input);

            // Keep incident vertices behind the reference face
            resultCount = 0;
            for (int i = 0; i < inCount; ++i)
            {
                double d = input[i].v.z - e.z;

                if (d <= 0)
                {
                    resultVerts[resultCount].v  = Transform.Mul(basis, input[i].v) + rPos;
                    resultVerts[resultCount].f  = input[i].f;
                    resultDepths[resultCount++] = d;
                }
            }


            Assert(resultCount <= 8);

            return(resultCount);
        }
コード例 #5
0
        internal void SynchronizeProxies()
        {
            BroadPhase broadphase = Scene.ContactManager.Broadphase;

            Tx.position = WorldCenter - Transform.Mul(Tx.rotation, LocalCenter);

            AABB      aabb;
            Transform tx = Tx;

            foreach (var box in Boxes)
            {
                box.ComputeAABB(tx, out aabb);
                broadphase.Update(box.broadPhaseIndex, aabb);
            }
        }
コード例 #6
0
ファイル: Box.cs プロジェクト: willnode/Qu3e-Sharp
        public void ComputeAABB(Transform tx, out AABB aabb)
        {
            Transform world = Transform.Mul(tx, local);

            Vec3 min = new Vec3(double.MaxValue, double.MaxValue, double.MaxValue);
            Vec3 max = new Vec3(-double.MaxValue, -double.MaxValue, -double.MaxValue);

            for (int i = 0; i < 8; ++i)
            {
                var v = Transform.Mul(world, Vec3.Mul(kBoxVertices[i], e));
                min = Vec3.Min(min, v);
                max = Vec3.Max(max, v);
            }

            aabb.min = min;
            aabb.max = max;
        }
コード例 #7
0
ファイル: Box.cs プロジェクト: willnode/Qu3e-Sharp
        public bool TestPoint(Transform tx, Vec3 p)
        {
            Transform world = Transform.Mul(tx, local);
            Vec3      p0    = Transform.MulT(world, p);

            for (int i = 0; i < 3; ++i)
            {
                double d  = p0[i];
                double ei = e[i];

                if (d > ei || d < -ei)
                {
                    return(false);
                }
            }

            return(true);
        }
コード例 #8
0
        // Resources:
        // http://www.randygaul.net/2014/05/22/deriving-obb-to-obb-intersection-sat/
        // https://box2d.googlecode.com/files/GDC2007_ErinCatto.zip
        // https://box2d.googlecode.com/files/Box2D_Lite.zip
        public static void BoxtoBox(Manifold m, Box a, Box b)
        {
            Transform atx = a.body.GetTransform();
            Transform btx = b.body.GetTransform();
            Transform aL  = a.local;
            Transform bL  = b.local;

            atx = Transform.Mul(atx, aL);
            btx = Transform.Mul(btx, bL);
            Vec3 eA = a.e;
            Vec3 eB = b.e;

            // B's frame input A's space
            Mat3 C = Mat3.Transpose(atx.rotation) * btx.rotation;

            Mat3   absC     = new Mat3();
            bool   parallel = false;
            double kCosTol  = 1e-6;

            for (int i = 0; i < 3; ++i)
            {
                for (int j = 0; j < 3; ++j)
                {
                    double val = Math.Abs(C[i][j]);
                    var    o   = absC[i];
                    o[j]    = val;
                    absC[i] = o;

                    if (val + kCosTol >= 1)
                    {
                        parallel = true;
                    }
                }
            }

            // Vector from center A to center B input A's space
            Vec3 t = Transform.MulT(atx.rotation, btx.position - atx.position);

            // Query states
            double s;
            double aMax  = -double.MaxValue;
            double bMax  = -double.MaxValue;
            double eMax  = -double.MaxValue;
            int    aAxis = ~0;
            int    bAxis = ~0;
            int    eAxis = ~0;
            Vec3   nA    = new Vec3();
            Vec3   nB    = new Vec3();
            Vec3   nE    = new Vec3();

            // Face axis checks

            // a's x axis
            s = Math.Abs(t.x) - (eA.x + Vec3.Dot(absC.Column0(), eB));
            if (TrackFaceAxis(ref aAxis, 0, s, ref aMax, atx.rotation.ex, ref nA))
            {
                return;
            }

            // a's y axis
            s = Math.Abs(t.y) - (eA.y + Vec3.Dot(absC.Column1(), eB));
            if (TrackFaceAxis(ref aAxis, 1, s, ref aMax, atx.rotation.ey, ref nA))
            {
                return;
            }

            // a's z axis
            s = Math.Abs(t.z) - (eA.z + Vec3.Dot(absC.Column2(), eB));
            if (TrackFaceAxis(ref aAxis, 2, s, ref aMax, atx.rotation.ez, ref nA))
            {
                return;
            }

            // b's x axis
            s = Math.Abs(Vec3.Dot(t, C.ex)) - (eB.x + Vec3.Dot(absC.ex, eA));
            if (TrackFaceAxis(ref bAxis, 3, s, ref bMax, btx.rotation.ex, ref nB))
            {
                return;
            }

            // b's y axis
            s = Math.Abs(Vec3.Dot(t, C.ey)) - (eB.y + Vec3.Dot(absC.ey, eA));
            if (TrackFaceAxis(ref bAxis, 4, s, ref bMax, btx.rotation.ey, ref nB))
            {
                return;
            }

            // b's z axis
            s = Math.Abs(Vec3.Dot(t, C.ez)) - (eB.z + Vec3.Dot(absC.ez, eA));
            if (TrackFaceAxis(ref bAxis, 5, s, ref bMax, btx.rotation.ez, ref nB))
            {
                return;
            }

            if (!parallel)
            {
                // Edge axis checks
                double rA;
                double rB;

                // Cross( a.x, b.x )
                rA = eA.y * absC[0][2] + eA.z * absC[0][1];
                rB = eB.y * absC[2][0] + eB.z * absC[1][0];
                s  = Math.Abs(t.z * C[0][1] - t.y * C[0][2]) - (rA + rB);
                if (TrackEdgeAxis(ref eAxis, 6, s, ref eMax, new Vec3(0, -C[0][2], C[0][1]), ref nE))
                {
                    return;
                }

                // Cross( a.x, b.y )
                rA = eA.y * absC[1][2] + eA.z * absC[1][1];
                rB = eB.x * absC[2][0] + eB.z * absC[0][0];
                s  = Math.Abs(t.z * C[1][1] - t.y * C[1][2]) - (rA + rB);
                if (TrackEdgeAxis(ref eAxis, 7, s, ref eMax, new Vec3(0, -C[1][2], C[1][1]), ref nE))
                {
                    return;
                }

                // Cross( a.x, b.z )
                rA = eA.y * absC[2][2] + eA.z * absC[2][1];
                rB = eB.x * absC[1][0] + eB.y * absC[0][0];
                s  = Math.Abs(t.z * C[2][1] - t.y * C[2][2]) - (rA + rB);
                if (TrackEdgeAxis(ref eAxis, 8, s, ref eMax, new Vec3(0, -C[2][2], C[2][1]), ref nE))
                {
                    return;
                }

                // Cross( a.y, b.x )
                rA = eA.x * absC[0][2] + eA.z * absC[0][0];
                rB = eB.y * absC[2][1] + eB.z * absC[1][1];
                s  = Math.Abs(t.x * C[0][2] - t.z * C[0][0]) - (rA + rB);
                if (TrackEdgeAxis(ref eAxis, 9, s, ref eMax, new Vec3(C[0][2], 0, -C[0][0]), ref nE))
                {
                    return;
                }

                // Cross( a.y, b.y )
                rA = eA.x * absC[1][2] + eA.z * absC[1][0];
                rB = eB.x * absC[2][1] + eB.z * absC[0][1];
                s  = Math.Abs(t.x * C[1][2] - t.z * C[1][0]) - (rA + rB);
                if (TrackEdgeAxis(ref eAxis, 10, s, ref eMax, new Vec3(C[1][2], 0, -C[1][0]), ref nE))
                {
                    return;
                }

                // Cross( a.y, b.z )
                rA = eA.x * absC[2][2] + eA.z * absC[2][0];
                rB = eB.x * absC[1][1] + eB.y * absC[0][1];
                s  = Math.Abs(t.x * C[2][2] - t.z * C[2][0]) - (rA + rB);
                if (TrackEdgeAxis(ref eAxis, 11, s, ref eMax, new Vec3(C[2][2], 0, -C[2][0]), ref nE))
                {
                    return;
                }

                // Cross( a.z, b.x )
                rA = eA.x * absC[0][1] + eA.y * absC[0][0];
                rB = eB.y * absC[2][2] + eB.z * absC[1][2];
                s  = Math.Abs(t.y * C[0][0] - t.x * C[0][1]) - (rA + rB);
                if (TrackEdgeAxis(ref eAxis, 12, s, ref eMax, new Vec3(-C[0][1], C[0][0], 0), ref nE))
                {
                    return;
                }

                // Cross( a.z, b.y )
                rA = eA.x * absC[1][1] + eA.y * absC[1][0];
                rB = eB.x * absC[2][2] + eB.z * absC[0][2];
                s  = Math.Abs(t.y * C[1][0] - t.x * C[1][1]) - (rA + rB);
                if (TrackEdgeAxis(ref eAxis, 13, s, ref eMax, new Vec3(-C[1][1], C[1][0], 0), ref nE))
                {
                    return;
                }

                // Cross( a.z, b.z )
                rA = eA.x * absC[2][1] + eA.y * absC[2][0];
                rB = eB.x * absC[1][2] + eB.y * absC[0][2];
                s  = Math.Abs(t.y * C[2][0] - t.x * C[2][1]) - (rA + rB);
                if (TrackEdgeAxis(ref eAxis, 14, s, ref eMax, new Vec3(-C[2][1], C[2][0], 0), ref nE))
                {
                    return;
                }
            }

            // Artificial axis bias to improve frame coherence
            double kRelTol = 0.95;
            double kAbsTol = 0.01;
            int    axis;
            double sMax;
            Vec3   n;
            double faceMax = Math.Max(aMax, bMax);

            if (kRelTol * eMax > faceMax + kAbsTol)
            {
                axis = eAxis;
                sMax = eMax;
                n    = nE;
            }

            else
            {
                if (kRelTol * bMax > aMax + kAbsTol)
                {
                    axis = bAxis;
                    sMax = bMax;
                    n    = nB;
                }

                else
                {
                    axis = aAxis;
                    sMax = aMax;
                    n    = nA;
                }
            }

            if (Vec3.Dot(n, btx.position - atx.position) < 0)
            {
                n = -n;
            }



            if (axis < 6)
            {
                Transform rtx;
                Transform itx;
                Vec3      eR;
                Vec3      eI;
                bool      flip;

                if (axis < 3)
                {
                    rtx  = atx;
                    itx  = btx;
                    eR   = eA;
                    eI   = eB;
                    flip = false;
                }

                else
                {
                    rtx  = btx;
                    itx  = atx;
                    eR   = eB;
                    eI   = eA;
                    flip = true;
                    n    = -n;
                }

                // Compute reference and incident edge information necessary for clipping

                ComputeIncidentFace(itx, eI, n, incident);
                Mat3 basis;
                Vec3 e;
                ComputeReferenceEdgesAndBasis(eR, rtx, n, axis, clipEdges, out basis, out e);

                // Clip the incident face against the reference face side planes
                int resultNum;
                resultNum = Clip(rtx.position, e, clipEdges, basis, incident, results, depths);

                if (resultNum != 0)
                {
                    m.contactCount = resultNum;
                    m.normal       = flip ? -n : n;

                    for (int i = 0; i < resultNum; ++i)
                    {
                        Contact c = m.contacts[i];

                        FeaturePair pair = results[i].f;

                        if (flip)
                        {
                            Swap(ref pair.inI, ref pair.inR);
                            Swap(ref pair.outI, ref pair.outR);
                        }

                        c.fp          = results[i].f;
                        c.position    = results[i].v;
                        c.penetration = depths[i];
                    }
                }
            }

            else
            {
                n = atx.rotation * n;

                if (Vec3.Dot(n, btx.position - atx.position) < 0)
                {
                    n = -n;
                }

                Vec3 PA, QA;
                Vec3 PB, QB;

                SupportEdge(atx, eA, n, out PA, out QA);

                SupportEdge(btx, eB, -n, out PB, out QB);

                Vec3 CA, CB;

                EdgesContact(out CA, out CB, PA, QA, PB, QB);

                m.normal       = n;
                m.contactCount = 1;

                Contact     c    = m.contacts[0];
                FeaturePair pair = new FeaturePair();
                pair.key      = axis;
                c.fp          = pair;
                c.penetration = sMax;
                c.position    = (CA + CB) * (0.5);
            }
        }
コード例 #9
0
        //--------------------------------------------------------------------------------------------------
        public static void ComputeIncidentFace(Transform itx, Vec3 e, Vec3 n, ClipVertex[] result)
        {
            n = -Transform.MulT(itx.rotation, n);
            Vec3 absN = Vec3.Abs(n);

            if (absN.x > absN.y && absN.x > absN.z)
            {
                if (n.x > 0)
                {
                    result[0].v.Set(e.x, e.y, -e.z);
                    result[1].v.Set(e.x, e.y, e.z);
                    result[2].v.Set(e.x, -e.y, e.z);
                    result[3].v.Set(e.x, -e.y, -e.z);

                    result[0].f.inI  = 9;
                    result[0].f.outI = 1;
                    result[1].f.inI  = 1;
                    result[1].f.outI = 8;
                    result[2].f.inI  = 8;
                    result[2].f.outI = 7;
                    result[3].f.inI  = 7;
                    result[3].f.outI = 9;
                }

                else
                {
                    result[0].v.Set(-e.x, -e.y, e.z);
                    result[1].v.Set(-e.x, e.y, e.z);
                    result[2].v.Set(-e.x, e.y, -e.z);
                    result[3].v.Set(-e.x, -e.y, -e.z);

                    result[0].f.inI  = 5;
                    result[0].f.outI = 11;
                    result[1].f.inI  = 11;
                    result[1].f.outI = 3;
                    result[2].f.inI  = 3;
                    result[2].f.outI = 10;
                    result[3].f.inI  = 10;
                    result[3].f.outI = 5;
                }
            }

            else if (absN.y > absN.x && absN.y > absN.z)
            {
                if (n.y > 0)
                {
                    result[0].v.Set(-e.x, e.y, e.z);
                    result[1].v.Set(e.x, e.y, e.z);
                    result[2].v.Set(e.x, e.y, -e.z);
                    result[3].v.Set(-e.x, e.y, -e.z);

                    result[0].f.inI  = 3;
                    result[0].f.outI = 0;
                    result[1].f.inI  = 0;
                    result[1].f.outI = 1;
                    result[2].f.inI  = 1;
                    result[2].f.outI = 2;
                    result[3].f.inI  = 2;
                    result[3].f.outI = 3;
                }

                else
                {
                    result[0].v.Set(e.x, -e.y, e.z);
                    result[1].v.Set(-e.x, -e.y, e.z);
                    result[2].v.Set(-e.x, -e.y, -e.z);
                    result[3].v.Set(e.x, -e.y, -e.z);

                    result[0].f.inI  = 7;
                    result[0].f.outI = 4;
                    result[1].f.inI  = 4;
                    result[1].f.outI = 5;
                    result[2].f.inI  = 5;
                    result[2].f.outI = 6;
                    result[3].f.inI  = 5;
                    result[3].f.outI = 6;
                }
            }

            else
            {
                if (n.z > 0)
                {
                    result[0].v.Set(-e.x, e.y, e.z);
                    result[1].v.Set(-e.x, -e.y, e.z);
                    result[2].v.Set(e.x, -e.y, e.z);
                    result[3].v.Set(e.x, e.y, e.z);

                    result[0].f.inI  = 0;
                    result[0].f.outI = 11;
                    result[1].f.inI  = 11;
                    result[1].f.outI = 4;
                    result[2].f.inI  = 4;
                    result[2].f.outI = 8;
                    result[3].f.inI  = 8;
                    result[3].f.outI = 0;
                }

                else
                {
                    result[0].v.Set(e.x, -e.y, -e.z);
                    result[1].v.Set(-e.x, -e.y, -e.z);
                    result[2].v.Set(-e.x, e.y, -e.z);
                    result[3].v.Set(e.x, e.y, -e.z);

                    result[0].f.inI  = 9;
                    result[0].f.outI = 6;
                    result[1].f.inI  = 6;
                    result[1].f.outI = 10;
                    result[2].f.inI  = 10;
                    result[2].f.outI = 2;
                    result[3].f.inI  = 2;
                    result[3].f.outI = 9;
                }
            }

            for (int i = 0; i < 4; ++i)
            {
                result[i].v = Transform.Mul(itx, result[i].v);
            }
        }
コード例 #10
0
        void CalculateMassData()
        {
            Mat3 inertia = Mat3.Diagonal(0);

            InvInertiaModel = Mat3.Diagonal(0);
            InvInertiaWorld = Mat3.Diagonal(0);
            InvMass         = 0;
            Mass            = 0;
            double mass = 0;

            if ((Flags & eStatic) > 0 || (Flags & eKinematic) > 0)
            {
                Vec3.Identity(ref LocalCenter);
                WorldCenter = Tx.position;
                return;
            }

            Vec3 lc = new Vec3();

            Vec3.Identity(ref lc);

            foreach (var box in Boxes)
            {
                if (box.density == 0)
                {
                    continue;
                }

                MassData md;
                box.ComputeMass(out md);
                mass    += md.mass;
                inertia += md.inertia;
                lc      += md.center * md.mass;
            }

            if (mass > 0)
            {
                Mass            = mass;
                InvMass         = 1 / mass;
                lc             *= InvMass;
                inertia        -= (Mat3.Identity * Vec3.Dot(lc, lc) - Mat3.OuterProduct(lc, lc)) * mass;
                InvInertiaModel = Mat3.Inverse(inertia);

                if ((Flags & eLockAxisX) > 0)
                {
                    Vec3.Identity(ref InvInertiaModel.ex);
                }

                if ((Flags & eLockAxisY) > 0)
                {
                    Vec3.Identity(ref InvInertiaModel.ey);
                }

                if ((Flags & eLockAxisZ) > 0)
                {
                    Vec3.Identity(ref InvInertiaModel.ez);
                }
            }
            else
            {
                // Force all dynamic bodies to have some mass
                InvMass         = 1;
                InvInertiaModel = Mat3.Diagonal(0);
                InvInertiaWorld = Mat3.Diagonal(0);
            }

            LocalCenter = lc;
            WorldCenter = Transform.Mul(Tx, lc);
        }
コード例 #11
0
 public Vec3 GetWorldVector(Vec3 v)
 {
     return(Transform.Mul(Tx.rotation, v));
 }
コード例 #12
0
 public Vec3 GetWorldPoint(Vec3 p)
 {
     return(Transform.Mul(Tx, p));
 }