Example #1
0
    /// <summary>
    /// PPHANTOM の周期(デフォルト1kHz)で繰り返し呼ばれるメソッド
    /// </summary>
    /// <description>この中ではPHANTOMの座標系/単位</description>
    /// <returns><c>true</c>, if update was phantomed, <c>false</c> otherwise.</returns>
    bool PhantomUpdate()
    {
        // 手先(ジンバル部分)の座標を取得 [mm]
        HandPosition = Phantom.GetPosition();

        // 手先姿勢を取得 [mm]
        HandRotation = Phantom.GetRotation();

        // PHANTOMの発揮力 [N]
        Vector3 force;

        // ターゲットから引き離す方向に力をかける
        //   今回、内容は適当。
        //   本当は Unity の物理演算結果から力を持ってきたい
        //   でもやり方分らない (>_<)
        Vector3 vec = TipPosition - TargetPosition;
        float   mag = vec.magnitude;

        // 0.05
        if (mag < 0.5f)
        {
            // 距離が 0.5 [mm] より目標に近いければ発揮力無しとする
            // 大きくなりすぎるのと、向きが不安定になるため
            force = Vector3.zero;
        }
        else
        {
            // 発揮力は目標に近づくと指数的に上昇
            float forceMag = ForceEfficent * Mathf.Log(Radius / mag) + ForceEfficent;
            if (forceMag < 0.0f)
            {
                forceMag = 0.0f;
            }

            // 安全のため最大発揮力を超える力はカット
            if (forceMag > MaxForce)
            {
                forceMag = MaxForce;
            }

            // ベクトルを正規化して、求めた大きさを掛ける
            force = vec * forceMag / mag;
        }

        // PHANTOMに対し力を指定
        Phantom.SetForce(force);

        return(true);
    }
Example #2
0
    /// <summary>
    /// PHANTOM の周期(デフォルト1kHz)で繰り返し呼ばれるメソッド
    /// </summary>
    /// <description>この中ではPHANTOMの座標系/単位</description>
    /// <returns><c>true</c>, if update was phantomed, <c>false</c> otherwise.</returns>
    bool PhantomUpdate()
    {
        // 手先(ジンバル部分)の座標を取得 [mm]
        HandPosition = Phantom.GetPosition();

        // 手先の速度を取得 [mm/s]
        HandVelocity = Phantom.GetVelocity();

        // 手先姿勢を取得
        HandRotation = Phantom.GetRotation();

        // ペン先端の座標を取得 [mm]
        TipPosition = Phantom.GetTipPosition();


        // PHANTOMの発揮力 [N]
        Vector3 force = Vector3.zero;

        // 球体から受ける力を計算
        if (SphereList != null)
        {
            foreach (PhantomRigidSphere sphere in SphereList)
            {
                force += sphere.CalculateForce(TipPosition, HandVelocity);
            }
        }

        // 力の上限を超えないようにする
        if (force.sqrMagnitude > (MaxForce * MaxForce))
        {
            force.Normalize();
            force *= MaxForce;
        }

        // PHANTOMに対し力を指定
        Phantom.SetForce(force);

        return(true);
    }