예제 #1
0
파일: TestOde2.cs 프로젝트: wachel/ode
    static OdeDynamic FindFirstDynamicFromSelfOrParent(Transform trans)
    {
        OdeDynamic dyn = trans.GetComponent <OdeDynamic>();

        if (dyn != null)
        {
            return(dyn);
        }
        else if (trans.parent != null)
        {
            return(FindFirstDynamicFromSelfOrParent(trans.parent));
        }
        return(null);
    }
예제 #2
0
파일: TestOde2.cs 프로젝트: wachel/ode
    void Start()
    {
        ode.dInitODE2();
        world = new Phy.World();
        space = new Phy.Space(world);

        world.gravity = new Vector3(0, -10, 0);

        List <OdeDynamic> dyns = FindAllCommant <OdeDynamic>(null);

        for (int i = 0; i < dyns.Count; i++)
        {
            OdeDynamic dyn = dyns[i];
            if (dyn.body == null)
            {
                dyn.body          = new Phy.Body(world);
                dyn.body.position = dyn.transform.position;
                dyn.body.rotation = dyn.transform.rotation;
                dyn.body.target   = dyn.transform;
                dyn.body.mass     = new Phy.Mass();
                if (dyn.isKinematic)
                {
                    dyn.body.SetKinematic();
                }
                bodies.Add(dyn.body);
            }
        }

        List <OdeJoint> joints = FindAllCommant <OdeJoint>(null);

        for (int i = 0; i < joints.Count; i++)
        {
            OdeJoint  jnt   = joints[i];
            Phy.Joint joint = Phy.Joint.Create(world, jnt.jointType);
            if (jnt.body0 && jnt.body0.body != null && jnt.body1 && jnt.body1.body != null)
            {
                joint.Attach(jnt.body0.body, jnt.body1.body);
                jnt.startTargetDir = jnt.transform.InverseTransformDirection((jnt.body1.transform.position - jnt.transform.position).normalized);
            }
            if (jnt.jointType == OdeJoint.JointType.Hinge)
            {
                Phy.HingeJoint hinge = (Phy.HingeJoint)joint;
                hinge.anchor = jnt.transform.position;
                hinge.axis   = jnt.transform.TransformDirection(Vector3.up);
                hinge.SetParam(ode.dParam.LoStop, Mathf.Min(jnt.loStop, jnt.hiStop) * Mathf.Deg2Rad);
                hinge.SetParam(ode.dParam.HiStop, Mathf.Max(jnt.loStop, jnt.hiStop) * Mathf.Deg2Rad);
            }
            else if (jnt.jointType == OdeJoint.JointType.Ball)
            {
                Phy.BallJoint ball = (Phy.BallJoint)joint;
                ball.anchor = jnt.transform.position;
            }
            else if (jnt.jointType == OdeJoint.JointType.Universal)
            {
                Phy.UniversalJoint universal = (Phy.UniversalJoint)joint;
                universal.anchor = jnt.transform.position;
                universal.anchor = jnt.transform.position;
                universal.axis1  = jnt.transform.TransformDirection(Vector3.up);
                universal.axis2  = jnt.transform.TransformDirection(Vector3.forward);
                universal.SetParam(ode.dParam.LoStop, Mathf.Min(jnt.loStop, jnt.hiStop) * Mathf.Deg2Rad);
                universal.SetParam(ode.dParam.HiStop, Mathf.Max(jnt.loStop, jnt.hiStop) * Mathf.Deg2Rad);
                universal.SetParam(ode.dParam.LoStop2, Mathf.Min(jnt.loStop2, jnt.hiStop2) * Mathf.Deg2Rad);
                universal.SetParam(ode.dParam.HiStop2, Mathf.Max(jnt.loStop2, jnt.hiStop2) * Mathf.Deg2Rad);
            }
            else if (jnt.jointType == OdeJoint.JointType.AMotor)
            {
                Phy.AMotor motor = (Phy.AMotor)joint;

                motor.SetMode(ode.dAMotorMode.Euler);
                motor.SetAxis(0, 1, new Vector3(0, 1, 0));
                motor.SetAxis(2, 2, new Vector3(0, 0, 1));
                //motor.SetAxis(2, 0, new Vector3(0, 0, 1));

                //motor.SetParam(ode.dParam.FMax , 0.200001f);
                //motor.SetParam(ode.dParam.FMax2, 0.200001f);
                motor.SetParam(ode.dParam.FMax3, 0.200001f);
                //motor.SetParam(ode.dParam.Vel , 0.0f);
                //motor.SetParam(ode.dParam.Vel2, 0.0f);
                motor.SetParam(ode.dParam.Vel3, 0.0f);
                motor.SetParam(ode.dParam.LoStop, Mathf.Min(jnt.loStop, jnt.hiStop) * Mathf.Deg2Rad);
                motor.SetParam(ode.dParam.HiStop, Mathf.Max(jnt.loStop, jnt.hiStop) * Mathf.Deg2Rad);
                motor.SetParam(ode.dParam.LoStop2, Mathf.Min(jnt.loStop2, jnt.hiStop2) * Mathf.Deg2Rad);
                motor.SetParam(ode.dParam.HiStop2, Mathf.Max(jnt.loStop2, jnt.hiStop2) * Mathf.Deg2Rad);
                motor.SetParam(ode.dParam.LoStop3, Mathf.Min(jnt.loStop3, jnt.hiStop3) * Mathf.Deg2Rad);
                motor.SetParam(ode.dParam.HiStop3, Mathf.Max(jnt.loStop3, jnt.hiStop3) * Mathf.Deg2Rad);
            }
            jnt.joint = joint;
        }

        List <OdeMuscle> ms = FindAllCommant <OdeMuscle>(null);

        for (int i = 0; i < ms.Count; i++)
        {
            OdeMuscle m = ms[i];
            if (m.body0 && m.body0.body != null && m.body1 && m.body1.body != null)
            {
                Vector3 relPos0 = Quaternion.Inverse(m.body0.transform.rotation) * (m.transform.position - m.body0.transform.position);
                Vector3 relPos1 = Quaternion.Inverse(m.body1.transform.rotation) * (m.tendon.transform.position - m.body1.transform.position);

                Phy.Muscle muscle = new Phy.Muscle();
                m.muscle       = muscle;
                muscle.body0   = m.body0.body;
                muscle.body1   = m.body1.body;
                muscle.relPos0 = relPos0;
                muscle.relPos1 = relPos1;
                muscle.Reset();

                muscles.Add(muscle);
            }
        }

        OdeCollider[] colliders = GameObject.FindObjectsOfType <OdeCollider>();
        for (int i = 0; i < colliders.Length; i++)
        {
            OdeCollider collider = colliders[i];
            Phy.Geom    geom     = collider.CreateGeom(space);
            OdeDynamic  dyn      = FindFirstDynamicFromSelfOrParent(collider.transform);
            if (dyn != null && dyn.body != null)
            {
                //ODE.Mass oldMass = dyn.body.GetMass();
                Phy.Mass mass = new Phy.Mass();
                collider.CalculateMass(mass, dyn.mass);
                //mass.Translate(collider.transform.position - dyn.transform.position);
                //oldMass.Add(mass);
                //dyn.body.SetMass(oldMass);
                if (!dyn.isKinematic)
                {
                    dyn.body.SetMass(mass);
                }

                geom.body = dyn.body;
                geom.offsetWorldPosition = collider.GetScaledOffsetPosition() + (collider.transform.position - dyn.transform.position);
                geom.offsetRotation      = collider.GetOffsetRotation();
            }
            else
            {
                geom.position = collider.transform.TransformPoint(collider.offsetPosition);
                geom.rotation = collider.transform.rotation * collider.GetOffsetRotation();
            }
            geoms.Add(geom);
        }
    }