双摆的程序实现

  1 // --------------------------------------------------------------------------------------
  2 
  3 inline float     rand2(float a, float b)
  4 {
  5     const float r = (float)(::rand() / ((float)RAND_MAX + 1));
  6     return r*(b-a) + a;
  7 }
  8 
  9 // --------------------------------------------------------------------------------------
 10 
 11 class IDoublePendulum
 12 {
 13 public:
 14 
 15     IDoublePendulum()
 16     {
 17         m_fGravity = 9.8f;
 18 
 19         m_m1 = 1.0f;
 20         m_m2 = 2.0f;
 21     
 22         m_l1 = 1.0f;
 23         m_l2 = 2.0f;
 24     }
 25 
 26     virtual ~IDoublePendulum() {}
 27 
 28     // 重置
 29     virtual void        Reset()
 30     {
 31         m_m1 = rand2(1.0f, 5.0f);
 32         m_m2 = rand2(1.0f, 5.0f);
 33 
 34         m_l1 = rand2(1.0f, 5.0f);
 35         m_l2 = rand2(1.0f, 5.0f);
 36     }
 37 
 38     // 按时更新
 39     virtual void        Update(float deltaTime) = NULL;
 40 
 41     // 重力系数
 42     virtual void        SetGravity(float g)
 43     {
 44         m_fGravity = g;
 45     }
 46 
 47     // 质量
 48     virtual void        SetMass1(float m)
 49     {
 50         m_m1 = m;
 51     }
 52 
 53     virtual void        SetMass2(float m)
 54     {
 55         m_m2 = m;
 56     }
 57 
 58     // 长度
 59     virtual void        SetLength1(float l)
 60     {
 61         m_l1 = l;
 62         UpdatePosition();
 63     }
 64 
 65     virtual void        SetLength2(float l)
 66     {
 67         m_l2 = l;
 68         UpdatePosition();
 69     }
 70 
 71     float               GetGravity() const
 72     {
 73         return m_fGravity;
 74     }
 75 
 76     float               GetMass1() const
 77     {
 78         return m_m1;
 79     }
 80 
 81     float               GetMass2() const
 82     {
 83         return m_m2;
 84     }
 85 
 86     float               GetLength1() const
 87     {
 88         return m_l1;
 89     }
 90 
 91     float               GetLength2() const
 92     {
 93         return m_l2;
 94     }
 95 
 96     const Vec3&         GetPosition1() const
 97     {
 98         return m_pos1;
 99     }
100 
101     const Vec3&         GetPosition2() const
102     {
103         return m_pos2;
104     }
105 
106 protected:
107     // 更新两球位置
108     virtual void        UpdatePosition() = NULL;
109 
110 protected:
111     float m_fGravity;   // 重力系数
112 
113     float m_m1, m_m2;   // 两球质量
114     float m_l1, m_l2;   // 两球距离
115 
116     Vec3 m_pos1, m_pos2;// 两球位置
117 };

 1 // --------------------------------------------------------------------------------------
 2 
 3 #ifndef _DoublePendulum01_H_
 4 #define _DoublePendulum01_H_
 5 
 6 // --------------------------------------------------------------------------------------
 7 
 8 #include "IDoublePendulum.h"
 9 
10 // --------------------------------------------------------------------------------------
11 
12 class DoublePendulum01 : public IDoublePendulum
13 {
14 public:
15     DoublePendulum01()
16     {
17         m_a1 = 1.0f;
18         m_a2 = 2.0f;    
19 
20         m_v1 = 0.0f;
21         m_v2 = 0.0f;
22 
23         m_w1 = m_a1;
24         m_w2 = m_a2;
25     }
26 
27     // 重置
28     void        Reset();
29 
30     // 按时更新
31     void        Update(float deltaTime);
32 
33 protected:
34     void        UpdatePosition()
35     {
36         m_pos1.x = m_l1*sinf(m_w1);
37         m_pos1.y = -m_l1*cosf(m_w1);
38         m_pos1.z = 0.0f;
39 
40         m_pos2.x = m_pos1.x + m_l2*sinf(m_w2);
41         m_pos2.y = m_pos1.y - m_l2*cosf(m_w2);
42         m_pos2.z = 0.0f;
43     }
44 
45 private:
46     float m_a1, m_a2;   // 两球初始角度
47     float m_w1, m_w2;   // 两球当前角度
48     float m_v1, m_v2;   // 两球的角加速度
49 };
50 
51 // --------------------------------------------------------------------------------------
52 
53 #endif

 1 // --------------------------------------------------------------------------------------
 2 
 3 #include "DoublePendulum01.h"
 4 
 5 // --------------------------------------------------------------------------------------
 6 
 7 /*
 8 求解线型方程组
 9 a*x + b*y + c = 0
10 d*x + e*y + f = 0
11 */
12 inline bool SolveLinalg(float a, float b, float c, float d, float e, float f, float& x, float& y)
13 {
14     float t = b*d - a*e;
15     if (fabs(t) < FLT_EPSILON)
16     {
17         x = 0.0f;
18         y = 0.0f;
19 
20         return false;
21     }
22 
23     x = (c*e - b*f)/t;
24     y = (a*f - c*d)/t;
25 
26     return true;
27 }
28 
29 // --------------------------------------------------------------------------------------
30 
31 void        DoublePendulum01::Reset()
32 {
33     IDoublePendulum::Reset();
34 
35     m_a1 = rand2(-2.0f, 2.0f);
36     m_a2 = rand2(-2.0f, 2.0f); 
37 
38     m_v1 = 0.0f;
39     m_v2 = 0.0f;
40 
41     m_w1 = m_a1;
42     m_w2 = m_a2;
43 
44     UpdatePosition();
45 }
46 
47 void        DoublePendulum01::Update(float deltaTime)
48 {
49     float a = m_l1*m_l1*(m_m1+m_m2);
50     float b = m_l1*m_m2*m_l2*cos(m_w1-m_w2);
51     float c = m_l1*(m_m2*m_l2*sin(m_w1-m_w2)*m_v2*m_v2 + (m_m1+m_m2)*m_fGravity*sin(m_w1));
52 
53     float d = m_m2*m_l2*m_l1*cos(m_w1-m_w2);
54     float e = m_m2*m_l2*m_l2;
55     float f = m_m2*m_l2*(-m_l1*sin(m_w1-m_w2)*m_v1*m_v1 + m_fGravity*sin(m_w2));
56 
57     // 角加速度
58     float dv1;
59     float dv2;
60     SolveLinalg(a, b, c, d, e, f, dv1, dv2);
61 
62     // 角速度
63     m_v1 += dv1*deltaTime;
64     m_v2 += dv2*deltaTime;
65 
66     // 角度
67     m_w1 += m_v1*deltaTime;
68     m_w2 += m_v2*deltaTime;
69 
70     UpdatePosition();
71 }