unity 单摆模拟

using UnityEngine;
using System.Collections;

public class Pendulum : MonoBehaviour {

    public Transform m_anchor; // 圆点
    public float g = 9.8f; // 重力加速度
    private Vector3 m_rotateAxis; // 旋转轴

    private float w = 0; // 角速度(单位：弧度/秒)

    private void Start() {
        // 求出旋转轴
        m_rotateAxis = Vector3.Cross(transform.position - m_anchor.position, Vector3.down);
    }

    private void Update() {
        float r = Vector3.Distance(m_anchor.position, transform.position);
        float l = Vector3.Distance(new Vector3(m_anchor.position.x, transform.position.y, m_anchor.position.z), transform.position);
        // 当钟摆摆动到另外一侧时，l为负，则角加速度alpha为负。
        Vector3 axis = Vector3.Cross(transform.position - m_anchor.position, Vector3.down);
        if (Vector3.Dot(axis, m_rotateAxis) < 0) {
            l = -l;
        }
        float sinA