예제 #1
0
        public PrismaticJoint(IWorldPool argWorld, PrismaticJointDef def)
            : base(argWorld, def)
        {
            LocalAnchorA = new Vec2(def.LocalAnchorA);
            LocalAnchorB = new Vec2(def.LocalAnchorB);
            LocalXAxisA = new Vec2(def.LocalAxisA);
            LocalXAxisA.Normalize();
            LocalYAxisA = new Vec2();
            Vec2.CrossToOutUnsafe(1f, LocalXAxisA, LocalYAxisA);
            ReferenceAngle = def.ReferenceAngle;

            Impulse = new Vec3();
            MotorMass = 0.0f;
            MotorImpulse = 0.0f;

            LowerTranslation = def.LowerTranslation;
            UpperTranslation = def.UpperTranslation;
            m_maxMotorForce = def.MaxMotorForce;
            m_motorSpeed = def.MotorSpeed;
            m_limitEnabled = def.EnableLimit;
            m_motorEnabled = def.EnableMotor;
            LimitState = LimitState.Inactive;

            K = new Mat33();
            Axis = new Vec2();
            Perp = new Vec2();
        }
예제 #2
0
        public PrismaticJoint(IWorldPool argWorld, PrismaticJointDef def)
            : base(argWorld, def)
        {
            m_localAnchorA = new Vec2(def.localAnchorA);
            m_localAnchorB = new Vec2(def.localAnchorB);
            m_localXAxisA = new Vec2(def.localAxisA);
            m_localXAxisA.normalize();
            m_localYAxisA = new Vec2();
            Vec2.crossToOutUnsafe(1f, m_localXAxisA, m_localYAxisA);
            m_referenceAngle = def.referenceAngle;

            m_impulse = new Vec3();
            m_motorMass = 0.0f;
            m_motorImpulse = 0.0f;

            m_lowerTranslation = def.lowerTranslation;
            m_upperTranslation = def.upperTranslation;
            m_maxMotorForce = def.maxMotorForce;
            m_motorSpeed = def.motorSpeed;
            m_enableLimit = def.enableLimit;
            m_enableMotor = def.enableMotor;
            m_limitState = LimitState.INACTIVE;

            m_K = new Mat33();
            m_axis = new Vec2();
            m_perp = new Vec2();
        }
예제 #3
0
 public static void Mul22ToOutUnsafe(Mat33 a, Vec2 v, Vec2 result)
 {
     Debug.Assert(v != result);
     result.Y = a.Ex.Y * v.X + a.Ey.Y * v.Y;
     result.X = a.Ex.X * v.X + a.Ey.X * v.Y;
 }
예제 #4
0
 public static void MulToOut(Mat33 a, Vec3 v, Vec3 result)
 {
     float tempy = v.X * a.Ex.Y + v.Y * a.Ey.Y + v.Z * a.Ez.Y;
     float tempz = v.X * a.Ex.Z + v.Y * a.Ey.Z + v.Z * a.Ez.Z;
     result.X = v.X * a.Ex.X + v.Y * a.Ey.X + v.Z * a.Ez.X;
     result.Y = tempy;
     result.Z = tempz;
 }
예제 #5
0
 public static Vec2 Mul22(Mat33 a, Vec2 v)
 {
     return new Vec2(a.Ex.X * v.X + a.Ey.X * v.Y, a.Ex.Y * v.X + a.Ey.Y * v.Y);
 }
예제 #6
0
 public static void Mul22ToOut(Mat33 a, Vec2 v, Vec2 result)
 {
     float tempx = a.Ex.X * v.X + a.Ey.X * v.Y;
     result.Y = a.Ex.Y * v.X + a.Ey.Y * v.Y;
     result.X = tempx;
 }
예제 #7
0
        // / Returns the zero matrix if singular.
        public void GetSymInverse33(Mat33 m)
        {
            float bx = Ey.Y * Ez.Z - Ey.Z * Ez.Y;
            float by = Ey.Z * Ez.X - Ey.X * Ez.Z;
            float bz = Ey.X * Ez.Y - Ey.Y * Ez.X;
            float det = Ex.X * bx + Ex.Y * by + Ex.Z * bz;
            if (det != 0.0f)
            {
                det = 1.0f / det;
            }

            float a11 = Ex.X, a12 = Ey.X, a13 = Ez.X;
            float a22 = Ey.Y, a23 = Ez.Y;
            float a33 = Ez.Z;

            m.Ex.X = det * (a22 * a33 - a23 * a23);
            m.Ex.Y = det * (a13 * a23 - a12 * a33);
            m.Ex.Z = det * (a12 * a23 - a13 * a22);

            m.Ey.X = m.Ex.Y;
            m.Ey.Y = det * (a11 * a33 - a13 * a13);
            m.Ey.Z = det * (a13 * a12 - a11 * a23);

            m.Ez.X = m.Ex.Z;
            m.Ez.Y = m.Ey.Z;
            m.Ez.Z = det * (a11 * a22 - a12 * a12);
        }
예제 #8
0
 // / Multiply a matrix times a vector.
 public static Vec3 Mul(Mat33 a, Vec3 v)
 {
     return new Vec3(v.X * a.Ex.X + v.Y * a.Ey.X + v.Z + a.Ez.X, v.X * a.Ex.Y + v.Y * a.Ey.Y + v.Z * a.Ez.Y, v.X * a.Ex.Z + v.Y * a.Ey.Z + v.Z * a.Ez.Z);
 }
예제 #9
0
        public void GetInverse22(Mat33 m)
        {
            float a = Ex.X, b = Ey.X, c = Ex.Y, d = Ey.Y;
            float det = a * d - b * c;
            if (det != 0.0f)
            {
                det = 1.0f / det;
            }

            m.Ex.X = det * d;
            m.Ey.X = (-det) * b;
            m.Ex.Z = 0.0f;
            m.Ex.Y = (-det) * c;
            m.Ey.Y = det * a;
            m.Ey.Z = 0.0f;
            m.Ez.X = 0.0f;
            m.Ez.Y = 0.0f;
            m.Ez.Z = 0.0f;
        }
예제 #10
0
 public static void MulToOutUnsafe(Mat33 a, Vec3 v, Vec3 result)
 {
     Debug.Assert(result != v);
     result.X = v.X * a.Ex.X + v.Y * a.Ey.X + v.Z * a.Ez.X;
     result.Y = v.X * a.Ex.Y + v.Y * a.Ey.Y + v.Z * a.Ez.Y;
     result.Z = v.X * a.Ex.Z + v.Y * a.Ey.Z + v.Z * a.Ez.Z;
 }
예제 #11
0
 public static void Mul22ToOutUnsafe(Mat33 a, Vec2 v, Vec2 result)
 {
     Debug.Assert(v != result);
     result.Y = a.Ex.Y * v.X + a.Ey.Y * v.Y;
     result.X = a.Ex.X * v.X + a.Ey.X * v.Y;
 }
예제 #12
0
 public static Vec2 Mul22(Mat33 a, Vec2 v)
 {
     return(new Vec2(a.Ex.X * v.X + a.Ey.X * v.Y, a.Ex.Y * v.X + a.Ey.Y * v.Y));
 }
예제 #13
0
 // / Multiply a matrix times a vector.
 public static Vec3 Mul(Mat33 a, Vec3 v)
 {
     return(new Vec3(v.X * a.Ex.X + v.Y * a.Ey.X + v.Z + a.Ez.X, v.X * a.Ex.Y + v.Y * a.Ey.Y + v.Z * a.Ez.Y, v.X * a.Ex.Z + v.Y * a.Ey.Z + v.Z * a.Ez.Z));
 }