Esempio n. 1
0
 QuaternionL(Vector3L v, FloatL w)
 {
     this.x = v.x;
     this.y = v.y;
     this.z = v.z;
     this.w = w;
 }
Esempio n. 2
0
    // 欧拉角转四元数
    static QuaternionL FromEulerRad(Vector3L euler)
    {
        FloatL yaw   = euler.z;
        FloatL pitch = euler.x;
        FloatL roll  = euler.y;

        FloatL yawOver2    = yaw * 0.5f;
        FloatL sinYawOver2 = FixPointMath.Sin(yawOver2);
        FloatL cosYawOver2 = FixPointMath.Cos(yawOver2);

        FloatL pitchOver2    = pitch * 0.5f;
        FloatL sinPitchOver2 = FixPointMath.Sin(pitchOver2);
        FloatL cosPitchOver2 = FixPointMath.Cos(pitchOver2);

        FloatL rollOver2    = roll * 0.5f;
        FloatL sinRollOver2 = FixPointMath.Sin(rollOver2);
        FloatL cosRollOver2 = FixPointMath.Cos(rollOver2);

        QuaternionL result;

        result.w = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2;
        result.x = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2;
        result.y = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2;
        result.z = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2;
        return(result);
    }
Esempio n. 3
0
    public static Vector3L ClosestPointOfPoint3dWithPlane3d(Vector3L point, Plane3d plane)
    {
        FloatL dot      = Vector3L.Dot(plane.m_planeNormal, point);
        FloatL distance = dot - plane.GetDistanceFromOrigin();

        return(point - plane.m_planeNormal * distance);
    }
Esempio n. 4
0
 static Vector3L NormalizeAngles(Vector3L angles)
 {
     angles.x = NormalizeAngle(angles.x);
     angles.y = NormalizeAngle(angles.y);
     angles.z = NormalizeAngle(angles.z);
     return(angles);
 }
Esempio n. 5
0
    public Vector3L ExtremePoint(Vector3L direction, ref FloatL projectionDistance)
    {
        Vector3L extremePoint = ExtremePoint(direction);

        projectionDistance = Vector3L.Dot(extremePoint, direction);
        return(extremePoint);
    }
Esempio n. 6
0
    public static Vector3L ClosestPointOfPoint3dWithLine3d(Vector3L point, Line3d line)
    {
        Vector3L lVec = line.m_point2 - line.m_point1; // Line Vector
        FloatL   t    = Vector3L.Dot(point - line.m_point1, lVec) / Vector3L.Dot(lVec, lVec);

        return(line.m_point1 + lVec * t);
    }
Esempio n. 7
0
    public static Matrix4x4L TRS(Vector3L pos, QuaternionL q, Vector3L s)
    {
        Matrix4x4L posMatrix = Matrix4x4L.identity;

        posMatrix.m03 = pos.x;
        posMatrix.m13 = pos.y;
        posMatrix.m23 = pos.z;

        Matrix4x4L rotateMatrix = Matrix4x4L.identity;

        rotateMatrix.m00 = 1 - 2 * q.y * q.y - 2 * q.z * q.z;
        rotateMatrix.m10 = 2 * q.x * q.y + 2 * q.w * q.z;
        rotateMatrix.m20 = 2 * q.x * q.z - 2 * q.w * q.y;

        rotateMatrix.m01 = 2 * q.x * q.y - 2 * q.w * q.z;
        rotateMatrix.m11 = 1 - 2 * q.x * q.x - 2 * q.z * q.z;
        rotateMatrix.m21 = 2 * q.y * q.z + 2 * q.w * q.x;

        rotateMatrix.m02 = 2 * q.x * q.z + 2 * q.w * q.y;
        rotateMatrix.m12 = 2 * q.y * q.z - 2 * q.w * q.x;
        rotateMatrix.m22 = 1 - 2 * q.x * q.x - 2 * q.y * q.y;

        Matrix4x4L scaleMatrix = Scale(s);

        Matrix4x4L ret = posMatrix * rotateMatrix * scaleMatrix;

        return(ret);
    }
Esempio n. 8
0
    public static Vector3L ClosestPointOfPoint3dWithTriangle3d(Triangle3d t, Vector3L p)
    {
        Plane3d  plane   = FromTriangle(t);
        Vector3L closest = ClosestPointOfPoint3dWithPlane3d(p, plane);

        // Closest point was inside triangle
        if (IntersectionTest3D.Point3dWithTriangle(closest, t))
        {
            return(closest);
        }

        Vector3L c1 = ClosestPointOfPoint3dWithSegment3d(closest, new Segment3d(t.m_point0, t.m_point1)); // Line AB
        Vector3L c2 = ClosestPointOfPoint3dWithSegment3d(closest, new Segment3d(t.m_point1, t.m_point2)); // Line BC
        Vector3L c3 = ClosestPointOfPoint3dWithSegment3d(closest, new Segment3d(t.m_point2, t.m_point0)); // Line CA

        FloatL magSq1 = (closest - c1).sqrMagnitude;
        FloatL magSq2 = (closest - c2).sqrMagnitude;
        FloatL magSq3 = (closest - c3).sqrMagnitude;

        if (magSq1 < magSq2 && magSq1 < magSq3)
        {
            return(c1);
        }
        else if (magSq2 < magSq1 && magSq2 < magSq3)
        {
            return(c2);
        }

        return(c3);
    }
Esempio n. 9
0
    public static void Test()
    {
        Debug.Log("IsLittleEndian " + BitConverter.IsLittleEndian);
        Debug.Log(FixPointMathTest.ConvertFloatToBinStr(1f));
        Debug.Log(FixPointMathTest.ConvertFloatToBinStr(17.625f));

        Debug.Log(FixPointMath.SqrtOld(new FloatL(0.955d)));
        Debug.Log(Mathf.Sqrt(0.955f));
        Debug.Log(FixPointMath.Sqrt(new FloatL(0.955d)));
        Vector3L dir    = new Vector3L(-0.093d, -0.093d, 0.988d);
        FloatL   angle1 = Vector3L_Angle(Vector3L.forward, dir);
        float    angle2 = Vector3_Angle(Vector3.forward, dir.Convert());

        Debug.Log("angle1 " + angle1 + " angle2 " + angle2);

        Debug.Log("sin 10 " + Mathf.Sin(10 * Mathf.Deg2Rad));
        Debug.Log("sinL 10 " + FixPointMath.Sin(10 * Mathf.Deg2Rad));

        Debug.Log("cos 10 " + Mathf.Cos(10 * Mathf.Deg2Rad));
        Debug.Log("cosL 10 " + FixPointMath.Cos(10 * Mathf.Deg2Rad));

        Debug.Log("acos 0.1 " + Mathf.Acos(0.1f));
        Debug.Log("acosL 0.1 " + FixPointMath.Acos(0.1f));

        Debug.Log("atan2 3/2 " + Mathf.Atan2(3, 2));
        Debug.Log("atan2L 3/2 " + FixPointMath.Atan2(3, 2));

        Debug.Log("asin 0.6 " + Mathf.Asin(0.6f));
        Debug.Log("asinL 0.6 " + FixPointMath.Asin(0.6f));
    }
    /// <summary>
    /// 注意abcd为绕序
    /// </summary>
    /// <param name="p"></param>
    /// <param name="a"></param>
    /// <param name="b"></param>
    /// <param name="c"></param>
    /// <param name="d"></param>
    /// <returns></returns>
    static bool IsInRectangle2d(Vector3L p, Vector3L a, Vector3L b, Vector3L c, Vector3L d)
    {
        Vector3L v11 = p - a;
        Vector3L v12 = b - a;

        Vector3L v21 = p - b;
        Vector3L v22 = c - b;

        Vector3L v31 = p - c;
        Vector3L v32 = d - c;

        Vector3L v41 = p - d;
        Vector3L v42 = a - d;

        Vector3L cross1 = Vector3L.Cross(v11, v12);
        Vector3L cross2 = Vector3L.Cross(v21, v22);
        Vector3L cross3 = Vector3L.Cross(v31, v32);
        Vector3L cross4 = Vector3L.Cross(v41, v42);

        if (Vector3L.Dot(cross1, cross2) > 0 &&
            Vector3L.Dot(cross2, cross3) > 0 &&
            Vector3L.Dot(cross3, cross4) > 0)
        {
            return(true);
        }
        else
        {
            return(false);
        }
    }
    /// <summary>
    /// 判断两条线段是否相交
    /// 现在有线段AB和线段CB
    //用线段AB的方向和C,D两点分别做差乘比较。如果C,D在同侧则return跳出
    //用线段CD的方向和A,B两点分别做差乘比较。如果A,B在同侧则return跳出
    //最终返回相交
    /// </summary>
    /// <param name="a"></param>
    /// <param name="b"></param>
    /// <param name="c"></param>
    /// <param name="d"></param>
    /// <returns></returns>
    static bool IsTwoSegmentIntersection(Vector2L a2d, Vector2L b2d, Vector2L c2d, Vector2L d2d)
    {
        Vector3L a = new Vector3L(a2d.x, 0, a2d.y);
        Vector3L b = new Vector3L(b2d.x, 0, b2d.y);
        Vector3L c = new Vector3L(c2d.x, 0, c2d.y);
        Vector3L d = new Vector3L(d2d.x, 0, d2d.y);

        var crossA = FixPointMath.Sign(Vector3L.Cross(d - c, a - c).y);
        var crossB = FixPointMath.Sign(Vector3L.Cross(d - c, b - c).y);

        if (FixPointMath.Approximately(crossA, crossB))
        {
            return(false);
        }

        var crossC = FixPointMath.Sign(Vector3L.Cross(b - a, c - a).y);
        var crossD = FixPointMath.Sign(Vector3L.Cross(b - a, d - a).y);

        if (FixPointMath.Approximately(crossC, crossD))
        {
            return(false);
        }

        return(true);
    }
    public static bool Line2dWithOrientedRectangle2d(Line2d line, OrientedRectangle2d rectangle2d)
    {
        Vector3L        forward  = RotateHelper.GetForward(rectangle2d.m_rotation);
        Vector3L        right    = Vector3L.Cross(Vector3L.up, forward);
        Vector3L        pos      = new Vector3L(rectangle2d.m_pos.x, 0, rectangle2d.m_pos.y);
        Vector3L        a        = pos + forward * rectangle2d.m_length * 0.5f + -right * rectangle2d.m_width * 0.5f;
        Vector3L        b        = pos + forward * rectangle2d.m_length * 0.5f + right * rectangle2d.m_width * 0.5f;
        Vector3L        c        = pos + -forward * rectangle2d.m_length * 0.5f + right * rectangle2d.m_width * 0.5f;
        Vector3L        d        = pos + -forward * rectangle2d.m_length * 0.5f + -right * rectangle2d.m_width * 0.5f;
        List <Vector2L> lineList = new List <Vector2L>();

        lineList.Add(new Vector2L(a.x, a.z));
        lineList.Add(new Vector2L(b.x, b.z));
        lineList.Add(new Vector2L(c.x, c.z));
        lineList.Add(new Vector2L(d.x, d.z));
        for (int i = 0; i < lineList.Count; ++i)
        {
            if (IsTwoSegmentIntersection(lineList[i], lineList[(i + 1) % lineList.Count], line.m_point1, line.m_point2))
            {
                return(true);
            }
        }

        return(false);
    }
    public static bool Circle2dWithOrientedRectangle2d(Circle2d circle, OrientedRectangle2d rectangle)
    {
        Vector3L        forward  = RotateHelper.GetForward(rectangle.m_rotation);
        Vector3L        right    = Vector3L.Cross(Vector3L.up, forward);
        Vector3L        pos      = new Vector3L(rectangle.m_pos.x, 0, rectangle.m_pos.y);
        Vector3L        a        = pos + forward * rectangle.m_length * 0.5f + -right * rectangle.m_width * 0.5f;
        Vector3L        b        = pos + forward * rectangle.m_length * 0.5f + right * rectangle.m_width * 0.5f;
        Vector3L        c        = pos + -forward * rectangle.m_length * 0.5f + right * rectangle.m_width * 0.5f;
        Vector3L        d        = pos + -forward * rectangle.m_length * 0.5f + -right * rectangle.m_width * 0.5f;
        List <Vector2L> lineList = new List <Vector2L>();

        lineList.Add(new Vector2L(a.x, a.z));
        lineList.Add(new Vector2L(b.x, b.z));
        lineList.Add(new Vector2L(c.x, c.z));
        lineList.Add(new Vector2L(d.x, d.z));
        for (int i = 0; i < lineList.Count; ++i)
        {
            Segment2d segment = new Segment2d();
            segment.m_point1 = lineList[i];
            segment.m_point2 = lineList[(i + 1) % lineList.Count];
            if (Segment2dWithCircle2d(segment, circle))
            {
                return(true);
            }
        }
        return(false);
    }
Esempio n. 14
0
    public static QuaternionL LookRotation(Vector3L forward, Vector3L up)
    {
        forward = Vector3L.Normalize(forward);
        Vector3L right = Vector3L.Normalize(Vector3L.Cross(up, forward));

        up = Vector3L.Cross(forward, right);
        FloatL m00 = right.x;
        FloatL m01 = right.y;
        FloatL m02 = right.z;
        FloatL m10 = up.x;
        FloatL m11 = up.y;
        FloatL m12 = up.z;
        FloatL m20 = forward.x;
        FloatL m21 = forward.y;
        FloatL m22 = forward.z;


        FloatL      num8       = (m00 + m11) + m22;
        QuaternionL quaternion = new QuaternionL();

        if (num8 > 0f)
        {
            FloatL num = FixPointMath.Sqrt(num8 + 1f);
            quaternion.w = num * 0.5f;
            num          = 0.5f / num;
            quaternion.x = (m12 - m21) * num;
            quaternion.y = (m20 - m02) * num;
            quaternion.z = (m01 - m10) * num;
            return(quaternion);
        }
        if ((m00 >= m11) && (m00 >= m22))
        {
            FloatL num7 = FixPointMath.Sqrt(((1f + m00) - m11) - m22);
            FloatL num4 = 0.5f / num7;
            quaternion.x = 0.5f * num7;
            quaternion.y = (m01 + m10) * num4;
            quaternion.z = (m02 + m20) * num4;
            quaternion.w = (m12 - m21) * num4;
            return(quaternion);
        }
        if (m11 > m22)
        {
            FloatL num6 = FixPointMath.Sqrt(((1f + m11) - m00) - m22);
            FloatL num3 = 0.5f / num6;
            quaternion.x = (m10 + m01) * num3;
            quaternion.y = 0.5f * num6;
            quaternion.z = (m21 + m12) * num3;
            quaternion.w = (m20 - m02) * num3;
            return(quaternion);
        }
        FloatL num5 = FixPointMath.Sqrt(((1f + m22) - m00) - m11);
        FloatL num2 = 0.5f / num5;

        quaternion.x = (m20 + m02) * num2;
        quaternion.y = (m21 + m12) * num2;
        quaternion.z = 0.5f * num5;
        quaternion.w = (m01 - m10) * num2;
        return(quaternion);
    }
 public static Vector3L ClampMagnitude(Vector3L vector, FloatL maxLength)
 {
     if (vector.sqrMagnitude > maxLength * maxLength)
     {
         return(vector.normalized * maxLength);
     }
     return(vector);
 }
Esempio n. 16
0
    public Vector3L MultiplyVector(Vector3L v)
    {
        Vector3L result;

        result.x = this.m00 * v.x + this.m01 * v.y + this.m02 * v.z;
        result.y = this.m10 * v.x + this.m11 * v.y + this.m12 * v.z;
        result.z = this.m20 * v.x + this.m21 * v.y + this.m22 * v.z;
        return(result);
    }
    public static Vector3L operator +(Vector3L a, Vector3L b)
    {
        Vector3L ret = new Vector3L();

        ret.x = a.x + b.x;
        ret.y = a.y + b.y;
        ret.z = a.z + b.z;
        return(ret);
    }
    public static Vector3L operator *(Vector3L a, FloatL d)
    {
        Vector3L ret = new Vector3L();

        ret.x = a.x * d;
        ret.y = a.y * d;
        ret.z = a.z * d;
        return(ret);
    }
    public static Vector3L operator /(Vector3L a, FloatL d)
    {
        Vector3L ret = new Vector3L();

        ret.x = a.x / d;
        ret.y = a.y / d;
        ret.z = a.z / d;
        return(ret);
    }
    public static Vector3L operator -(Vector3L a)
    {
        Vector3L ret = new Vector3L();

        ret.x = -a.x;
        ret.y = -a.y;
        ret.z = -a.z;
        return(ret);
    }
Esempio n. 21
0
    /// <summary>
    /// 3d空间中点到直线距离
    /// </summary>
    /// <param name="a"></param>
    /// <param name="b"></param>
    /// <param name="s"></param>
    /// <returns></returns>
//     public static FloatL DistanceOfPoint3dWithLine3d(Line3d line3d, Vector3L s)
//     {
//         FloatL ab = FixPointMath.Sqrt(FixPointMath.Pow((line3d.m_point1.x - line3d.m_point2.x), 2.0f) + FixPointMath.Pow((line3d.m_point1.y - line3d.m_point2.y), 2.0f) + FixPointMath.Pow((line3d.m_point1.z - line3d.m_point2.z), 2.0f));
//         FloatL as2 = FixPointMath.Sqrt(FixPointMath.Pow((line3d.m_point1.x - s.x), 2.0f) + FixPointMath.Pow((line3d.m_point1.y - s.y), 2.0f) + FixPointMath.Pow((line3d.m_point1.z - s.z), 2.0f));
//         FloatL bs = FixPointMath.Sqrt(FixPointMath.Pow((s.x - line3d.m_point2.x), 2.0f) + FixPointMath.Pow((s.y - line3d.m_point2.y), 2.0f) + FixPointMath.Pow((s.z - line3d.m_point2.z), 2.0f));
//         FloatL cos_A = (FixPointMath.Pow(as2, 2.0f) + FixPointMath.Pow(ab, 2.0f) - FixPointMath.Pow(bs, 2.0f)) / (2 * ab * as2);
//         FloatL sin_A = FixPointMath.Sqrt(1 - FixPointMath.Pow(cos_A, 2.0f));
//         return as2 * sin_A;
//     }
    public static Vector3L ClosestPointOfPoint3dWithSegment3d(Vector3L point, Segment3d line)
    {
        Vector3L lVec = line.m_point2 - line.m_point1; // Line Vector
        FloatL   t    = Vector3L.Dot(point - line.m_point1, lVec) / Vector3L.Dot(lVec, lVec);

        t = FixPointMath.Max(t, 0.0f); // Clamp to 0
        t = FixPointMath.Min(t, 1.0f); // Clamp to 1
        return(line.m_point1 + lVec * t);
    }
    public static bool Segment2dWithCircle2d(Segment2d segment, Circle2d circle)
    {
        Segment3d segment3d = new Segment3d(
            new Vector3L(segment.m_point1.x, 0, segment.m_point1.y),
            new Vector3L(segment.m_point2.x, 0, segment.m_point2.y));
        Vector3L closestPoint = Distance3d.ClosestPointOfPoint3dWithSegment3d(new Vector3L(circle.m_pos.x, 0, circle.m_pos.y), segment3d);
        Vector2L distance     = circle.m_pos - new Vector2L(closestPoint.x, closestPoint.z);

        return(distance.magnitude <= circle.m_radius);
    }
Esempio n. 23
0
    public OBB3d(Vector3L pos, QuaternionL qua, Vector3L size)
    {
        m_pos      = pos;
        m_rotation = qua;

        //m_xLength = size.x;
        //m_yLength = size.y;
        //m_zLength = size.z;
        m_size = size;
    }
    public override bool Equals(object other)
    {
        if (!(other is Vector3L))
        {
            return(false);
        }
        Vector3L vector = (Vector3L)other;

        return(this.x.Equals(vector.x) && this.y.Equals(vector.y) && this.z.Equals(vector.z));
    }
    public static Vector3L Project(Vector3L vector, Vector3L onNormal)
    {
        FloatL num = Vector3L.Dot(onNormal, onNormal);

        if (num < FloatL.Epsilon)
        {
            return(Vector3L.zero);
        }
        return(onNormal * Vector3L.Dot(vector, onNormal) / num);
    }
    public static Vector3L Normalize(Vector3L value)
    {
        FloatL num = Vector3L.Magnitude(value);

        if (num > FloatL.Epsilon)
        {
            return(value / num);
        }
        return(Vector3L.zero);
    }
Esempio n. 27
0
 public OBB3d(Vector3L pos, QuaternionL qua, FloatL xLength, FloatL yLength, FloatL zLength)
 {
     m_pos      = pos;
     m_rotation = qua;
     //m_xLength = xLength;
     //m_yLength = yLength;
     //m_zLength = zLength;
     m_size.x = xLength;
     m_size.y = yLength;
     m_size.z = zLength;
 }
Esempio n. 28
0
    public static FloatL Vector3L_Angle(Vector3L from, Vector3L to)
    {
        Debug.Log("x " + to.normalized.x + " y " + to.normalized.y + " z " + to.normalized.z);
        FloatL dot = Vector3L.Dot(from.normalized, to.normalized);

        Debug.Log("dotL " + dot);
        FloatL acos = FixPointMath.Acos(FixPointMath.Clamp(dot, -1f, 1f));

        Debug.Log("acosL " + acos);
        return(acos * 57.29578d);
    }
    public static Vector3L MoveTowards(Vector3L current, Vector3L target, FloatL maxDistanceDelta)
    {
        Vector3L a         = target - current;
        FloatL   magnitude = a.magnitude;

        if (magnitude <= maxDistanceDelta || magnitude == 0f)
        {
            return(target);
        }
        return(current + a / magnitude * maxDistanceDelta);
    }
    public Vector3L ExtremePoint(Vector3L direction)
    {
        Vector3L maxPoint = GetMax();
        Vector3L minPoint = GetMin();

        return(new Vector3L(
                   direction.x >= 0 ? maxPoint.x : minPoint.x,
                   direction.y >= 0 ? maxPoint.y : minPoint.y,
                   direction.z >= 0 ? maxPoint.z : minPoint.z
                   ));
    }