SimpleSimulationEngine/cpp/common/dynamics/Body2D.h at master · ProkopHapala/SimpleSimulationEngine · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
#ifndef Body2D_h
#define Body2D_h

#include <math.h>
#include <cstdlib>
#include <stdio.h>

#include "fastmath.h"
#include "Vec2.h"

class KinematicBody2D {
	public:
	double phi;
	Vec2d  rot;
	Vec2d  pos;
	inline void setAngle( double phi_ ){  phi = phi_; rot.fromAngle( phi_ ); }
};

class PointBody2D{ public:
	// parameters
	double mass;
	// auxiliary parameters
	double invMass;
	// State variables
	Vec2d pos;
	Vec2d vel;
	// auxiliary variables
	Vec2d force;

	// ==== function declarations

	virtual void evalForce();
	virtual void move( double dt );
	//virtual void draw( );

	// ==== inline functions

	inline void setMass( double mass_ ){
		mass    = mass_;
		invMass = 1 / mass;
	};

	inline void move_PointBody2D( double dt ){
		vel.add_mul( force, dt*invMass );
		pos.add_mul( vel,   dt );
	};

	inline void clean_temp( ){  force.set(0.0); }

	PointBody2D(){};

};

class RigidBody2D : public PointBody2D { public:
	static constexpr double ROT_NORM2_PREC = 1e-6;
	// parameters
	double	I;
	// auxiliary parameters
	double	invI;
	// State variables
	double phi;
	double omega;
	// auxiliary variables
	Vec2d   rot;
	double torq;

	int shape; // displayList

	// ==== function declarations

	void from_mass_points( int n, double * amass, Vec2d * apos );
	virtual void move( double dt );
	//virtual void draw(           );
	//virtual void draw_shape(     );

	// ==== inline functions

    inline void setI       ( double I_   ){ I = I_; invI = 1 / I;                               };
	inline void setAngle   ( double phi_ ){	phi = phi_;		         rot.fromAngle( phi_ ); 	}
	inline void sinc_rot   (             ){	rot.x = cos( phi );		 rot.y = sin( phi );	    }
	inline bool check_rot  (             ){	double r2 = rot.norm2(); return ( fabs( r2 - 1.0d ) > ROT_NORM2_PREC );	}
    inline void clean_temp (             ){ force.set(0.0);          torq=0;                    }
	inline void setDefaults(             ){	omega = 0.0;		     clean_temp();	            }

	inline void move_RigidBody2D( double dt ){
		// postion
		vel.add_mul( force, dt*invMass );
		pos.add_mul( vel,   dt );
		// rotation
		omega       += torq  * invI * dt;
		double dphi  = omega * dt;
		phi         += dphi;
		rot.rotate_taylor2( dphi );
		//rot.x = cos(phi); rot.y = sin(phi);
	};

	inline void apply_force( const Vec2d& dforce, const Vec2d& gdpos ){
		torq += gdpos.cross( dforce );
		force.add( dforce );
	};

	inline void apply_anchor( double k, const Vec2d& lpos, const Vec2d& gpos0 ){
		Vec2d sforce, gdpos;
		gdpos.set_mul_cmplx( rot, lpos );
		sforce.set_add( gdpos, pos );
		sforce.sub( gpos0 );
		sforce.mul( -k );
		apply_force  (  sforce, gdpos );
		//printf( " gdpos %f %f \n", gdpos.x, gdpos.y );
		//drawLine( gpos0, gdpos + pos  );
	};

};

class SpringConstrain2D{
	public:
	Vec2d p1,p2;
	RigidBody2D *b1,*b2;
	double k;

	// ==== function declarations

	void apply();
	//void draw();
	SpringConstrain2D( double k_, RigidBody2D* b1_, RigidBody2D* b2_, const Vec2d& p1_, const Vec2d& p2_ );

};

#endif