Morph-SynRJ

view phy_element.h @ 0:22de913c2d84

find changesets by author, revision, files, or words in the commit message
morph-synrj intro
author "Cedric Pinson <cedric.pinson@alcove.fr> <mornifle@plopbyte.net>"
date Tue Nov 27 15:23:52 2007 +0100 (2007-11-27)
parents
children
line source
1 /** @file phy_element.h
2 *
3 * @brief element base class
4 *
5 *****************************************************************************
6 *
7 * @author psc80
8 *
9 * @date Created 2003/02
10 *
11 * @version $Id: phy_element.h,v 1.5 2004/01/14 20:36:54 psc80 Exp $
12 *
13 ****************************************************************************/
14
15 #ifndef PHY_ELEMENT_H
16 #define PHY_ELEMENT_H
17
18
19 #include "sys_assert.h"
20 #include "mth_vector3.h"
21
22
23 namespace phy {
24
25 class Element_t
26 {
27
28 mth::Vector3_t position;
29 mth::Vector3_t force;
30 mth::Vector3_t velocity;
31 float mass;
32
33 bool blocked;
34
35 void Init() {
36 position.Init();
37 force.Init();
38 velocity.Init();
39 mass=1;
40 blocked=false;
41 }
42
43 public:
44
45
46 Element_t() { Init();}
47 ~Element_t() {}
48
49 void Blocked() { blocked=true;}
50 bool IsBlocked() { return blocked;}
51
52 void Mass(float _m) { mass=_m;}
53 float Mass() const { return mass;}
54
55 void Position(const mth::Vector3_t& _pos) { position=_pos;}
56 const mth::Vector3_t& Position() const { return position;}
57 mth::Vector3_t& Position() { return position;}
58
59 void InitForce() { force.Init();}
60 void ApplyForce(const mth::Vector3_t& _f) { force+=_f;}
61
62
63 mth::Vector3_t& Velocity() { return velocity;}
64 const mth::Vector3_t& Velocity() const { return velocity;}
65 void Velocity(const mth::Vector3_t& _vel) {velocity=_vel;}
66 void Update(double _dt) {
67 SYS_ASSERT(mass>0);
68 mth::Vector3_t acceleration=force/mass;
69 velocity+=acceleration*_dt;
70 position+=velocity*_dt;
71 }
72
73 };
74
75
76
77 class SpringDamper_t {
78
79 Element_t* link[2];
80
81 float ks,kd;
82 float l;
83
84
85 void Init() {
86 link[0]=link[1]=0;
87 ks=kd=0;
88 }
89
90 public:
91
92 SpringDamper_t() { Init(); }
93
94 void Init(float _ks, float _kd,float _l) { ks=_ks;kd=_kd;l=_l;}
95 void Init(Element_t* _e0, Element_t* _e1) { link[0]=_e0;link[1]=_e1;}
96
97 void Update(/*double _dt*/) {
98
99 SYS_ASSERT(link[0] && link[1]);
100 mth::Vector3_t L=link[0]->Position()-link[1]->Position();
101 mth::Vector3_t vel=link[0]->Velocity()-link[1]->Velocity();
102
103 float magL=L.MagSqr();
104 if (magL>1e-5)
105 magL=mth::Sqrt(magL);
106 //L*=1.0/magL;
107
108 float springTension=((magL-l)*(ks));
109 // mth::Vector3_t force=L*((magL-l)*(-ks));
110
111 float val=(springTension + ( (vel*L)*kd / magL ))/magL ;
112 //val=mth::Sign(val)*mth::Abs(mth::Min(50,val));
113 mth::Vector3_t force= L * (-val);
114
115 if (!link[0]->IsBlocked())
116 link[0]->ApplyForce(force);
117 if (!link[1]->IsBlocked())
118 link[1]->ApplyForce(-force);
119
120 }
121
122 };
123
124
125 }
126
127 #endif