PHJointPid.h

#ifndef PH_JOINTPID_H
#define PH_JOINTPID_H

#include <Physics/PHJoint1D.h>
#include <Physics/PHJointMulti.h>

namespace Spr{;

class PHJointPid:public SGBehaviorEngine{
protected:
    /// ‘O‰ñ‚Ì–Ú•W’l
    float lastGoal;
    /// Ï•ª
    float integratedError;
public:
    SGOBJECTDEF(PHJointPid);
    /// §Œä‘Ώۊ֐ß
    UTRef<PHJointBase> joint;
    /// ‘Ώۂ̎²
    int axis;
    /// –Ú•W’l      Step()‚ðŒÄ‚Ô‘O‚ɐݒ肷‚éD
    float goal;
    /// –Ú•W‚Ì‘¬“x  Step()‚ðŒÄ‚Ô‘O‚ɐݒ肷‚éD
    float dGoal;
    /// PID‚ÌŒW”
    float proportional, differential, integral;
    /// –Ú•W‚̃^ƒCƒv 0:ˆÊ’u§Œä 1F‘¬“x§Œä
    int type;
    /// proportional,differential,integral‚É‚æ‚éƒgƒ‹ƒN(PID§Œä)
    float pTorque, dTorque, iTorque;

    /// p_torque, d_torque,i_torque‚̎擾
    float GetProportionalTorque(){ return pTorque; }
    float GetDifferentialTorque(){ return dTorque; }
    float GetIntegralTorque(){ return iTorque; }

    /// –Ú•W’l,–Ú•W‚Ì‘¬“x   Step()‚ðŒÄ‚Ô‘O‚ɐݒ肷‚é
    void SetPGoal(float p_goal){ goal = p_goal;}
    void SetDGoal(float d_goal){ dGoal = d_goal;}
    
    /// 
    PHJointPid():type(0), proportional(0), differential(0), integral(0), goal(0), dGoal(FLT_MAX), lastGoal(0), integratedError(0), axis(0){}
    ///
    void Print(std::ostream& os) const;
    ///
    static PHJointPid* Find(PHJoint1D* j, SGScene* scene);
    /// ƒvƒ‰ƒCƒIƒŠƒeƒB
    virtual int GetPriority() const { return SGBP_FORCEGENERATOR; }
    /// ƒV[ƒ“‚ðƒNƒŠƒA‚·‚é‚Æ‚«‚̏ˆ—
    virtual void Clear(SGScene* s){ joint=NULL; }

    /// ó‘Ԃ̓ǂݏo‚µ
    virtual void LoadState(const SGBehaviorStates& states);
    /// ó‘Ô‚Ì•Û‘¶
    virtual void SaveState(SGBehaviorStates& states) const;
    /// PID§Œä‚ð‚©‚¯‚éD
    void Step(SGScene* s);  

    ///
    double GetJointTorque(){ return joint->GetJointTorque(axis); }

    size_t NReferenceObjects();
    SGObject* ReferenceObject(size_t i);
    bool AddChildObject(SGObject* o, SGScene* s);
    bool DelChildObject(SGObject* o, SGScene* s);
protected:
};



/*  ‰Â“®ˆæ‚ɂ‚¢‚Ä
    ƒjƒ…[ƒgƒ‰ƒ‹‚ð‚Ü‚Á‚·‚®‚Æ‚µ‚āC
    - ƒÆEƒÓ‚̃Oƒ‰ƒt‚ʼn“®ˆæ‚ð•\‚·DÜ‚êü‹ßŽ—D
    - ÅŒã‚ÌŽ²‚Í,’萔 or ’†S‚Æ ƒÆEƒÓ‚̐ݒè“_‚ɂ‚¢‚ĐݒèD
    - ƒÆEƒÓ‚͈ᔽ‚µ‚½‚çCÅŠñ‚̃ƁEƒÓ‚ÉŒü‚©‚Á‚ÄPID§ŒäD
ƒR[ƒ“‚Å—Ç‚¢àD
ƒR[ƒ“‚Ì’†S‚ÌŒü‚«Vec3f ‚ÆŠp“xƒÆ‚¾‚¯‚Å‚æ‚¢D
*/
class PHJointBallPid:public SGBehaviorEngine{
protected:
    /// ‘O‰ñ‚̃S[ƒ‹
    Quaterniond lastGoal;
    /// Ï•ª
    Vec3f integratedError;
public:
    SGOBJECTDEF(PHJointBallPid);
    /// §Œä‘Ώۊ֐ß
    UTRef<PHJointBall> joint;
    /// –Ú•W’l          Step()‚ðŒÄ‚Ô‘O‚ɐݒ肷‚éD
    Quaternionf goal;
    /// –Ú•W’l‚Ì”÷•ª    Step()‚ðŒÄ‚Ô‘O‚ɐݒ肷‚éDÝ’肵‚È‚­‚Ä‚à—Ç‚¢D
    Vec3f dGoal;
    /// PID‚ÌŒW”
    float proportional, differential, integral;
    /// proportional,differential,integral‚É‚æ‚éƒgƒ‹ƒN(PID§Œä)
    Vec3f pTorque, dTorque, iTorque;

    /// p_torque, d_torque,i_torque‚̎擾
    Vec3f GetProportionalTorque(){ return pTorque; }
    Vec3f GetDifferentialTorque(){ return dTorque; }
    Vec3f GetIntegralTorque(){ return iTorque; }
    
    /// 
    PHJointBallPid():proportional(0), differential(0), integral(0), dGoal(FLT_MAX,0,0){}
    ///
    static PHJointBallPid* Find(PHJointBall* j, SGScene* scene);
    /// ƒvƒ‰ƒCƒIƒŠƒeƒB
    virtual int GetPriority() const { return SGBP_FORCEGENERATOR; }
    /// ƒV[ƒ“‚ðƒNƒŠƒA‚·‚é‚Æ‚«‚̏ˆ—
    virtual void Clear(SGScene* s){ joint=NULL; }

    /// ó‘Ԃ̓ǂݏo‚µ
    virtual void LoadState(const SGBehaviorStates& states);
    /// ó‘Ô‚Ì•Û‘¶
    virtual void SaveState(SGBehaviorStates& states) const;
    /// PID§Œä‚ð‚©‚¯‚éD
    void Step(SGScene* s);

    ///
    Vec3f GetJointTorque(){ return joint->GetTorque(); }

    size_t NReferenceObjects();
    SGObject* ReferenceObject(size_t i);
    bool AddChildObject(SGObject* o, SGScene* s);
    bool DelChildObject(SGObject* o, SGScene* s);   

    virtual void Print(std::ostream& os) const;

};


}

#endif

---