1 #include "Constraint.h"
#include "matrix/matrix_io.h"
#include <math.h>
#include <iostream>
//#define CONSTRAINT_DB
#ifndef PR
#define PR( x ) std::cout << #x " = "<< std::endl << ( x ) << endl
#endif
////////////////////////////////
// Constraint Methods
/////////////////////////////////
15 Constraint::Constraint( ConstraintSystem& sys, unsigned int id )
: cs( sys ), idnum( id ) {}
18 Constraint::Constraint( const Constraint& c ) :
cs( c.cs ), idnum( c.idnum ) {}
const Constraint&
22 Constraint::operator=( const Constraint & c ){
if( this != &c ) idnum = c.idnum;
return *this;
}
27 Constraint::~Constraint( ){}
29 inline USHORT Constraint::id( ) {
return idnum;
}
33 inline void Constraint::id( USHORT i ) {
idnum = i;
}
/////////////////////////////////////
// Distance Methods
/////////////////////////////////////
44 Distance::Distance( Particle& a, Particle& b,
45 ConstraintSystem & sys, unsigned int id, float dist ) :
Constraint( sys, id ), p1( a ), p2( b ), distance( dist ) {
distance = dist;
}
51 Distance::Distance( Distance & d ):
Constraint( d.cs, d.idnum ), p1( d.p1 ),
p2( d.p2 ), distance( d.distance ) {}
56 const Distance& Distance::operator=( const Distance & d ) {
return *this;
}
61 QDataStream& Distance:: operator<<( QDataStream& qds ) const {
qds << ConstraintSystem::DISTANCE;
qds << p1.id( ) << p2.id( ) << idnum << distance;
return qds;
}
67 void Distance::buildFromFile( ParticleSystem* ps, Constraint*& d,
68 ConstraintSystem& sys, QDataStream& qds ){
Particle *pcl1, *pcl2; USHORT id; float dist;
int i; qds >> i;
pcl1 = ps->getParticle( i ); qds >> i;
pcl2 = ps->getParticle( i );
qds >> id; qds >> dist;
d = new Distance( *pcl1, *pcl2, sys, id, dist );
}
79 inline float Distance::length( ) {
return distance;
}
83 inline void Distance::length( float f ) {
distance = f;
}
88 float Distance::U( ) {
cs.sum2 = ( ( cs.t[0] )*( cs.t[0] )
+ ( cs.t[1] )*( cs.t[1] )
+ ( cs.t[2] )*( cs.t[2] ) );
return sqrt( cs.sum2 );
}
96 float Distance::c_func( ) {
return ( cs.u - distance );
}
101 float Distance::c_func_dot( ) {
cs.t[6] = ( cs.t[0] )*( cs.t[3] );
cs.t[7] = ( cs.t[1] )*( cs.t[4] );
cs.t[8] = ( cs.t[2] )*( cs.t[5] );
cs.sum = ( cs.t[6] ) + ( cs.t[7] )+ ( cs.t[8] );
return ( cs.sum / cs.u );
}
110 void Distance::fill_J( ) {
//cout << "entering Distance::fill_J( )" << std::endl;
//cout << "entering Distance::fill_J( )" << std::endl;
// all the results are put in the same numbered
// row as the idnum. They are put in thec ol
// corresponding to the particles id.
//std::cout << "*( cs.J ): \n"<<*( cs.J ) << std::endl;
try{
for( USHORT i = 0; i < 3;i++ ) {
float val = ( cs.t[i] )/ ( cs.u );
cs.J->setElem( val, idnum, ( p1.id( )*3 )+i );
cs.J->setElem( -val, idnum, ( p2.id( )*3 )+i );
}
}
catch( Matrix::Error &e ) {
std::cout <<"Matrix Exception encountered" <<std::endl;
std::cout << e.message( )<<std::endl;
exit( 1 );
}
//std::cout << "entering Distance::fill_J( )" << std::endl;
}
132 void Distance::fill_J_dot( ) {
//std::cout << "entering Distance::fill_J_dot( )" << std::endl;
float val1, val2, val3, val4;
for( USHORT i = 0; i < 3;i++ ) {
val1 = ( cs.t[i+3] ) / cs.u;
val2 = ( cs.t[i] )*( cs.sum );
val3 = val2 /( pow( cs.sum2, 1.5 ) );
val4 = val1 - val3;
cs.Jdot->setElem( val4, idnum, ( p1.id( )*3 )+i );
cs.Jdot->setElem( -val4, idnum, ( p2.id( )*3 )+i );
}
//std::cout << "leaving Distance::fill_J_dot( )" << std::endl;
}
147 void Distance::calculate( ) {
//std::cout << "entering Distance::calculate( )" << std::endl;
static int pos = Particle::POSN;
static int vel = Particle::VEL;
cs.t[0] = ( p1.x[pos] - p2.x[pos] );
cs.t[1] = ( p1.x[pos+1] - p2.x[pos+1] );
cs.t[2] = ( p1.x[pos+2] - p2.x[pos+2] );
cs.t[3] = p1.x[vel] - p2.x[vel];
cs.t[4] = p1.x[vel+1] - p2.x[vel+1];
cs.t[5] = p1.x[vel+2] - p2.x[vel+2];
cs.u = U( ); // bit that goes on the bottom
//cout << "calulate: cs.u= " << cs.u << endl;
cs.C->setElem( c_func( ), idnum );
cs.Cdot->setElem( c_func_dot( ), idnum );
#ifdef CONSTRAINT_DB
PR( *( cs.C ) ); PR( *( cs.Cdot ) );
#endif
fill_J( );
fill_J_dot( );
}
173 bool Distance::usesParticle( const Particle* p )const {
if( p == &p1 || p == &p2 ) return true;
else return false;
}
/////////////////////////////////
// Methods for class Circle
// and class Circle::Data ( just a data struct thing
/////////////////////////////////
184 Circle::Data::Data( float xval, float yval, float zval, float rval ):
x( xval ), y( yval ), z( zval ), r( rval*rval )
{}
189 Circle::Data::Data( const Data & d ):
x( d.x ), y( d.y ), z( d.z ), r( d.r ){}
193 const Circle::Data & Circle::Data::operator=( const Data & d ){
if ( this != &d ) {
x=d.x; y=d.y; z=d.z; r=d.r;
}
return *this;
}
201 Circle::Circle( Particle &p1, ConstraintSystem& sys, unsigned int id,
202 Data& data ): Constraint( sys, id ) , d( data ), p( p1 ){}
205 QDataStream& Circle::operator<<( QDataStream& qds ) const {
qds << ConstraintSystem::CIRCLE;
qds << p.id( ) << idnum;
qds << d.x << d.y << d.z << sqrtf( d.r );
return qds;
}
213 void Circle::buildFromFile( ParticleSystem* ps, Constraint*& pc,
214 ConstraintSystem& sys, QDataStream& qds ){
int pid; USHORT id; Particle* pp;
qds >> pid; pp = ps->getParticle( pid );
qds >> id;
float x, y, z, r;
qds >> x >> y >> z >> r;
Data dt( x, y, z, r );
pc = new Circle( *pp, sys, id, dt );
}
227 void Circle::calculate( ) {
cs.t[0] = ( p.x[Particle::POSN] ) - d.x;
cs.t[1] = ( p.x[Particle::POSN+1] ) - d.y;
cs.t[2] = ( p.x[Particle::POSN+2] ) - d.z;
cs.t[4] = ( cs.t[0] )*( cs.t[0] ) + ( cs.t[1] )*( cs.t[1] )
+ ( cs.t[2] )*( cs.t[2] );
cs.C->setElem( ( cs.t[4] - d.r ), idnum );
cs.t[5] = 2.0f*( ( cs.t[0] )*( p.x[Particle::VEL] )
+ ( cs.t[1] )*( p.x[Particle::VEL+1] )
+ ( cs.t[2] )*( p.x[Particle::VEL+2] ) );
cs.Cdot->setElem( cs.t[5], idnum );
for( USHORT i= 0; i < 3 ;i++ ) {
cs.J->setElem( 2.0*( cs.t[i] ), idnum, ( p.id( )*3 )+i );
}
for( USHORT i= 0; i < 3 ;i++ ) {
cs.Jdot->setElem( 2.0*( p.x[Particle::VEL+i] ),
idnum, ( p.id( )*3 )+i );
}
}
255 const Circle::Data & Circle::getData( )const {
return d;
}
260 bool Circle::usesParticle( const Particle* pc )const {
if( &p == pc ) return true;
else return false;
}
///////////////////////////////
// Dimension Methods
269 Dimension::Dimension( Particle& pl, ConstraintSystem& sys,
unsigned int id, int dimension ):
Constraint( sys, id ), p( pl ), dim( dimension ) {
posn = pl.x[Particle::POSN+dim];
}
277 Dimension::Dimension( const Dimension & h ):
Constraint( h.cs, h.idnum ), posn( h.posn ), p( h.p ),
dim( h.dim ){}
282 void Dimension::calculate( ) {
float c = p.x[Particle::POSN+dim] - posn;
cs.C->setElem( c, idnum );
c = p.x[Particle::VEL+dim];
cs.Cdot->setElem( c, idnum );
cs.J->setElem( 1.0f, idnum, ( p.id( )*3 )+dim );
}
294 bool Dimension::usesParticle( const Particle* pc )const {
if( &p == pc ) return true;
else return false;
}
300 QDataStream& Dimension::operator<<( QDataStream& qds ) const {
qds << ConstraintSystem::DIMENSION;
qds << p.id( ) << idnum << dim;
return qds;
}
307 void Dimension::buildFromFile( ParticleSystem* ps, Constraint*& pc,
308 ConstraintSystem& sys, QDataStream& qds ){
int p; Particle* pp;
qds >> p; pp = ps->getParticle( p );
USHORT id, d;
qds >> id >> d;
pc = new Dimension( *pp, sys, id, d );
}
///////////////////////////////
// ConstraintSystem Methods
322 ConstraintSystem::ConstraintSystem( ParticleSystem & p ):
ps( p ) {
constraints = new std::vector<Constraint*>( );
t = new float[9];
C = Cdot = 0;J = Jdot = Jt = LHS= 0;Q = final = 0;
M = 0;q = qdot = lamda = b= 0;
kd = ks = 0.5;
changed = true;
}
333 ConstraintSystem::~ConstraintSystem( ){
for ( USHORT i = 0 ; i < constraints->size( ); i++ ) {
delete ( ( *constraints )[i] );
}
delete constraints;
delete C; delete Cdot; delete Q;
delete J; delete Jdot; delete Jt;
delete M; delete q; delete b; delete LHS;
delete qdot; delete lamda; delete final;
delete [] t;
}
348 void ConstraintSystem::addDistConstraint( Particle& p1,
350 Particle& p2, float length ) {
Constraint* c = new Distance( p1, p2, *this,
constraints->size( ), length );
constraints->push_back( c );
changed = true;
}
359 void ConstraintSystem::addDimensionConstraint( Particle &p, int dim ) {
Constraint* c = new Dimension( p, *this, constraints->size( ), dim );
constraints->push_back( c );
changed = true;
}
366 void ConstraintSystem::addCircleConstraint
367 ( Particle& p, Circle::Data& d ){
Constraint* c = new Circle( p, *this, constraints->size( ), d );
constraints->push_back( c );
changed = true;
}
375 void ConstraintSystem::fixParticle( Particle &p ){
for( int i = 0; i<3; i++ ) addDimensionConstraint( p, i );
}
382 void ConstraintSystem::clear( ) {
for ( USHORT i = 0 ; i < constraints->size( ); i++ ) {
delete ( ( *constraints )[i] );
}
constraints->clear( );
changed = true;
}
392 bool ConstraintSystem::removeParticleRefs( const Particle *p ){
bool alter = false; bool b = false;
Iter i = constraints->begin( );
while( i != constraints->end( ) ) {
b = ( *i )->usesParticle( p );
if( b ) {
delete *i;
i = constraints->erase( i );
alter = true;
}
else i++;
}
if( alter ) {
// renumber the constraints
Iter i = constraints->begin( ); int num = 0;
while( i != constraints->end( ) )
( *( i++ ) )->id( num++ );
}
if( changed || alter ) changed = true;
return alter;
}
417 void ConstraintSystem::adjustForces( ) {
//cout << "entering adjustForces"<< endl;
if( constraints->size( ) == 0 || ps.size( ) == 0 ) return;
bool partres = ps.isChanged( );
if( changed || partres ) initialize( partres );
// now fill q and qdot with the required values
Particle *p = 0;
for ( USHORT i = 0; i < ps.size( ); i++ ) {
p = ps[i];
q->setElem( p->x[Particle::POSN], 3*i );
q->setElem( p->x[Particle::POSN+1], ( 3*i )+1 );
q->setElem( p->x[Particle::POSN+2], ( 3*i )+2 );
qdot->setElem( p->x[Particle::VEL], 3*i );
qdot->setElem( p->x[Particle::VEL+1], ( 3*i )+1 );
qdot->setElem( p->x[Particle::VEL+2], ( 3*i )+2 );
Q->setElem( p->x[Particle::FORCE], 3*i );
Q->setElem( p->x[Particle::FORCE+1], 3*i+1 );
Q->setElem( p->x[Particle::FORCE+2], 3*i+2 );
}
#ifdef CONSTRAINT_DB
PR( *q ); PR( *qdot ); PR( *Q );
#endif
// now get the constraint objects to fill
// the J and Jdot matrices
J->zero( );
Jdot->zero( );
for ( USHORT i = 0; i < constraints->size( );i++ )
( ( *constraints )[i] )->calculate( );
// solve the matrix eqn.
*C = ks* ( *C );
*Cdot = kd * ( *Cdot );
*b = ( ( -1.0f )*( ( *Jdot )*( *qdot ) ) ) - ( ( *J )*( *Q ) ) - ( *C ) - ( *Cdot );
#ifdef CONSTRAINT_DB
PR( *C ); PR( *Cdot );
PR( *b ); PR( *J );
#endif
*Jt = *J;
Jt->transpose( );
*LHS = ( *J )*( *Jt );
#ifdef CONSTRAINT_DB
PR( *Jt ); PR( *LHS );
#endif
ChSolve( ( *LHS ), ( *lamda ), ( *b ) );
// and finally.
*final = ( *Jt )*( *lamda );
#ifdef CONSTRAINT_DB
PR( *final ); PR( *lamda );
#endif
//redistribute to force vectors.
for( unsigned int i = 0;i < ps.size( );i++ ) {
( ps[i] )->x[Particle::FORCE] += final->getElem( i*3 );
( ps[i] )->x[Particle::FORCE+1] += final->getElem( i*3+1 );
( ps[i] )->x[Particle::FORCE+2] += final->getElem( i*3+2 );
}
changed = false;
}
// Constructs all Matrices and Vectors to the
//correct dimemsions.
492 void ConstraintSystem::initialize( bool psChanged ) {
#ifdef CONSTRAINT_DB
std::cout <<"entering initialize" << std::endl;
#endif
// the size of the vectors q and qdot ( and M ) only depend
// on the number of particles, not the number of
// constraints.
// the size of C, Cdot and lamda only depend on the number
// of constraints, not the number of particles
// the size of J , Jt and Jdot depend on both the number of
// particles and the number of constraints.
USHORT p = ( USHORT )( 3 * ps.size( ) );
USHORT c = ( USHORT ) constraints->size( );
if( psChanged ) {
// we need to change the size of q, qdot, M
delete q; q = new Vectf( p );
delete qdot; qdot = new Vectf( p );
delete Q; Q = new Vectf( p );
delete final; final = new Vectf( p );
// M matrix not used at the moment.
}
if( changed ) {
delete C; C = new Vectf( c );
delete Cdot; Cdot = new Vectf( c );
delete lamda; lamda = new Vectf( c );
delete b; b = new Vectf( c );
delete LHS; LHS = new Matrixf( c, c );
}
delete J; J = new Matrixf( c, p );
delete Jdot; Jdot = new Matrixf( c, p );
delete Jt; Jt = new Matrixf( p, c );
}
533 const Constraint* ConstraintSystem::getConstraint( unsigned int index )const {
if ( index >= constraints->size( ) ) return 0;
return ( *constraints )[index];
}
540 unsigned int ConstraintSystem::size( ) const {
return constraints->size( );
}
// uses the same particleSystem as it always had,
// so watch out.
546 void ConstraintSystem::loadFromFile( QDataStream& qds ){
clear( );
Constraint* pc = 0; bool exit = false;
ConstraintType in;
while ( !exit ) {
qds >> in;
switch( in ) {
case END :
exit = true; pc = 0;
break;
case DISTANCE :
Distance::buildFromFile( &ps, pc, *this, qds );
break;
case CIRCLE :
Circle::buildFromFile( &ps, pc, *this, qds );
break;
case DIMENSION :
Dimension::buildFromFile( &ps, pc, *this, qds );
break;
default:
std::cout << "ConstraintSystem::loadFromFile : "<<
"fallen thru case!!"<< std::endl;
}
if( pc ) constraints->push_back( pc );
pc = 0;
}
changed = true;
}
//--------------------------------------------------------
// Global IO
//--------------------------------------------------------
581 QDataStream& operator<<( QDataStream& qds,
const ConstraintSystem::ConstraintType& ct ){
qds << ( int ) ct; return qds;
}
587 QDataStream& operator>>( QDataStream& qds,
ConstraintSystem::ConstraintType& ct ){
int i; qds >> i;
ct = static_cast<ConstraintSystem::ConstraintType>( i );
return qds;
}
596 QDataStream& operator<<( QDataStream& qds, const Constraint& c ){
c.operator<<( qds );
return qds;
}
601 QDataStream& operator<<( QDataStream& qds,
const ConstraintSystem& cs ) {
qds << ParticleSystem::CONSTRAINTSYSTEM ;
ConstraintSystem::Iter i = cs.constraints->begin( );
for( ;i != cs.constraints->end( );i++ ) {
qds << **i;
}
qds << ConstraintSystem::END;
return qds;
}
1
#ifndef cl_constraint
#define cl_constraint
#include "matrix/Matrix.h"
//#include "matrix/matrix_io.h"
#include "Particle.h"
#include <vector>
#include <qdatastream.h>
typedef Matrix::Vector<float> Vectf ;
typedef Matrix::LMatrix<float> Matrixf;
15 class ConstraintSystem;
16 class Particle;
17 class ParticleSystem;
20 class Constraint {
// A constraint has four calculations to make
// 1. The value of the constraint function , C
// 2. The value of the derivative of the constraint
// function, Cdot.
// 3. Its block in J 4. Its block in Jdot
public:
30 Constraint( ConstraintSystem& sys, unsigned int id = 0 );
31 Constraint( const Constraint& c );
32 const Constraint& operator=( const Constraint& c );
33 virtual ~Constraint( );
34 virtual QDataStream& operator<<( QDataStream& ) const = 0;
// the main function. places data in C, Cdot,
// J, Jdot;
38 virtual void calculate( ) = 0;
// returns true if the Constraint contains
// a pointer to the particle.
42 virtual bool usesParticle( const Particle* p ) const = 0;
44 USHORT id( );
45 void id( USHORT i );
protected:
48 ConstraintSystem & cs;
49 USHORT idnum;
};
55 class Distance : public Constraint {
public:
58 Distance( Particle& a,
59 Particle &b, ConstraintSystem& sys,
unsigned int id , float dist = 100 );
61 Distance( Distance &d );
62 virtual ~Distance( ){}
63 virtual QDataStream& operator<<( QDataStream& ) const;
//call this function and get back a brand spanking
// new Distance pointed to by the argument
67 static void buildFromFile( ParticleSystem*, Constraint*&,
68 ConstraintSystem&, QDataStream& );
69 float length( );
70 void length( float f );
72 void calculate( );
73 const Particle& getParticle1( ) const {return p1;}
74 const Particle& getParticle2( ) const {return p2;}
75 virtual bool usesParticle( const Particle* p ) const;
private:
79 const Distance& operator= ( const Distance & d );
80 Particle & p1;
81 Particle & p2;
float distance;
84 float c_func( );
85 float c_func_dot( );
86 void fill_J( );
87 void fill_J_dot( );
88 float U( ); //for UGLY!
};
////////////////////////////////////////
// class Circle
// For keeping a particle on a circle ( duh! )
////////////////////////////////////////
98 class Circle : public Constraint {
public:
100 class Data;
102 Circle( Particle& p1, ConstraintSystem& sys, unsigned int id,
103 Data& data );
104 virtual ~Circle( ){}
105 virtual QDataStream& operator<<( QDataStream& ) const;
106 static void buildFromFile( ParticleSystem*, Constraint*&,
107 ConstraintSystem&, QDataStream& );
109 const Data & getData( ) const ;
110 void calculate( );
112 virtual bool usesParticle( const Particle* pc ) const ;
115 class Data{
public:
117 Data( float xval, float yval, float zval, float rval );
118 Data( const Data & d );
119 const Data & operator=( const Data & d );
float x, y, z, r;
122 friend QDataStream& operator<<( QDataStream&,
const Data& );
};
private:
128 Data d;
129 Particle & p;
};
////////////////////////////////////////
// class Dimension
// For keeping a particle in a given plane
////////////////////////////////////////
141 class Dimension : public Constraint {
public:
//Dimension ={0, 1, 2}
144 Dimension( Particle& pl, ConstraintSystem& sys,
unsigned int id, int dimension );
147 Dimension( const Dimension & f );
148 virtual ~Dimension( ){}
150 void calculate( );
151 virtual bool usesParticle( const Particle* pc ) const;
152 virtual QDataStream& operator<<( QDataStream& ) const;
153 static void buildFromFile( ParticleSystem*, Constraint*&,
154 ConstraintSystem&, QDataStream& );
private:
float posn;
159 Particle & p;
160 USHORT dim;
162 const Dimension& operator= ( const Dimension & f );
163 float c_func_dot( );
164 void fill_J( );
165 void fill_J_dot( );
};
172 class ConstraintSystem {
public:
176 ConstraintSystem( ParticleSystem & p );
177 ~ConstraintSystem( );
179 friend class Constraint;
180 friend class Distance;
181 friend class Circle;
182 friend class Dimension;
183 void addDistConstraint( Particle & p1, Particle& p2, float length );
184 void addDimensionConstraint( Particle &p, int dim );
185 void addCircleConstraint( Particle& p, Circle::Data& d );
186 void fixParticle( Particle &p );
187 void clear( );
188 bool removeParticleRefs( const Particle* p );
189 const Constraint * getConstraint( unsigned int index ) const ;
190 unsigned int size( ) const ;
191 void adjustForces( );
float u, sum, sum2;
typedef std::vector<Constraint*>::iterator Iter;
//enumeration for building system from file.
enum ConstraintType{END, DISTANCE, CIRCLE, DIMENSION};
197 friend QDataStream& operator<<( QDataStream&,
const ConstraintSystem& );
199 void loadFromFile( QDataStream& qds );
private:
201 void initialize( bool psChanged );
204 ParticleSystem & ps;
205 std::vector<Constraint*> * constraints;
206 Vectf* C;
207 Vectf* Cdot;
208 Matrixf* J;
209 Matrixf* Jt; // J transpose
210 Matrixf* Jdot;
211 Matrixf* M; // mass Matrix
212 Matrixf* LHS;
213 Vectf* q; //position vector
214 Vectf* qdot; //velocity vector
215 Vectf* Q; //force vector
216 Vectf *lamda;
217 Vectf* b;
218 Vectf* final;
float kd;
float ks;
float* t; //array for Distance to do calculations with
224 bool changed;
};
228 QDataStream& operator<<( QDataStream&,
const ConstraintSystem::ConstraintType& );
230 QDataStream& operator>>( QDataStream&,
ConstraintSystem::ConstraintType& );
232 QDataStream& operator<<( QDataStream&, const Constraint& );
#endif
1 #include "ConstraintChooser.h"
3 ConstraintChooser::ConstraintChooser( QWidget * parent,
const char * name ): QDialog( parent, name ){
setCaption( "Add Constraint" );
QGridLayout * g = new QGridLayout( this, 2, 2, 6 );
list = new QListBox( this, "list" );
list->setMinimumHeight( 80 );
input = new QLineEdit( this, "input" );
make = new QPushButton( "Make Constraint", this, "make" );
g->addWidget( list, 1, 1 );
g->addWidget( make, 2, 1 );
g->addWidget( input, 1, 2 ) ;
list->setFocusPolicy( QWidget::StrongFocus );
connect( make, SIGNAL( clicked( ) ), this, SLOT( getInput( ) ) );
f = -1.0;
}
23 void ConstraintChooser::numParticlesSelected( unsigned int n ) {
list->clear( );
switch( n ) {
case 0 :input->setEnabled( false );
return;
case 2 : list->insertItem( "Distance" );
input->setEnabled( true );
addMost( );
break ;
case 1 : list->insertItem( "Circle" );
input->setEnabled( true );
addMost( );
break;
default: addMost( );
input->setEnabled( false );
}
}
42 void ConstraintChooser::addMost( ) {
list->insertItem( "Fixed" );
list->insertItem( "Horizontal Plane" );
list->insertItem( "Vertical Plane" );
}
48 void ConstraintChooser::getInput( ) {
QString s = list->currentText( );
if( s == "Fixed" )
emit makeConstraint( FIXED, 0 );
else if( s == "Horizontal Plane" )
emit makeConstraint( HORIZ, 0 );
else if( s == "Vertical Plane" )
emit makeConstraint( VERT, 0 );
else if( s == "Circle" ){
if( getNumInput( ) )
emit makeConstraint( CIRCLE, f );
}
else if( s == "Distance" ) makeDistance( );
}
64 bool ConstraintChooser::getNumInput( ) {
QString s = input->text( );
bool b = false;
f = s.toFloat( &b );
if( b && f < 0.0 ) b = false; // no negatives
return b ;
}
73 void ConstraintChooser::makeDistance( ) {
QString s = input->text( );
if( s.isEmpty( ) )
emit makeConstraint( DEF_DISTANCE, 0 );
else if( getNumInput( ) )
emit makeConstraint( DISTANCE, f );
}
1 #include <qdialog.h>
#include <qlistbox.h>
#include <qlineedit.h>
#include <qpushbutton.h>
#include <iostream>
#include <qlayout.h>
#ifndef CON_CHOOSER
#define CON_CHOOSER
enum ConstraintType {HORIZ, VERT, FIXED, DISTANCE, CIRCLE,
DEF_DISTANCE};
14 class ConstraintChooser : public QDialog {
Q_OBJECT
public:
ConstraintChooser( QWidget * parent = 0, const char * name = 0 );
public slots:
void numParticlesSelected( unsigned int n );
void getInput( );
signals:
void makeConstraint( ConstraintType c, float data );
private:
QListBox *list;
QLineEdit *input;
QPushButton *make;
//QPushButton* cancel;
void addMost( );
bool getNumInput( );
void makeDistance( );
float f;
};
#endif
1
#include "CustomSpinBox.h"
//----------------------------------
// ZeroToOne Methods
//----------------------------------
9 ZeroToOne::ZeroToOne( QWidget *parent, const char *name ) :
QSpinBox( parent, name ){
connect( this, SIGNAL( valueChanged( int ) ), this, SLOT( notify( int ) ) );
}
15 QString ZeroToOne::mapValueToText( int value ) {
float f = ( float ) value;
return QString( "%1" ).arg( f/100 );
}
21 int ZeroToOne::mapTextToValue( bool*ok ) {
return int( text( ).toFloat( )*100 );
}
26 void ZeroToOne::notify( int val ) {
bool b = true;
emit valueChangedf( ( ( float )mapTextToValue( &b ) )/100 );
}
32 void ZeroToOne::setValuef( float val ) {
float ok;
if( val < 0.0 ) ok = 0;
else if( val > 1.0 ) ok = 1.0;
else ok = val;
setValue( ( int ) ( ok * 100.0 ) );
updateDisplay( );
}
//----------------------------------
// ZeroTo500 Methods
//----------------------------------
45 ZeroTo500::ZeroTo500( QWidget *parent, const char *name ):
QSpinBox( parent, name ){
connect( this, SIGNAL( valueChanged( int ) ),
this, SLOT( notify( int ) ) );
}
52 void ZeroTo500::setValuef( float val ) {
float ok;
if( val < 0.0 ) ok = 0;
else if( val > 500.0 ) ok = 500.0;
else ok = val;
setValue( ( int ) ( ok / 5.0 ) );
updateDisplay( );
}
62 QString ZeroTo500::mapValueToText( int value ) {
float f = ( float ) value;
return QString( "%1" ).arg( f*5 );
}
68 int ZeroTo500::mapTextToValue( bool*ok ) {
return int( text( ).toFloat( )/5.0 );
}
73 void ZeroTo500::notify( int val ) {
bool b = true;
emit valueChangedf( ( ( float )mapTextToValue( &b ) )*5.0 );
}
//----------------------------------
// ZeroToTen Methods
//----------------------------------
84 ZeroToTen::ZeroToTen( QWidget *parent, const char *name ):
QSpinBox( parent, name ){
connect( this, SIGNAL( valueChanged( int ) ),
this, SLOT( notify( int ) ) );
}
91 void ZeroToTen::setValuef( float val ) {
float ok;
if( val < 0.0 ) ok = 0.0;
else if( val > 10.0 ) ok = 10.0;
else ok = val;
setValue( ( int ) ( ok * 10.0 ) );
updateDisplay( );
}
101 QString ZeroToTen::mapValueToText( int value ) {
float f = ( float ) value;
return QString( "%1" ).arg( f / 10.0 );
}
107 int ZeroToTen::mapTextToValue( bool*ok ) {
return int( text( ).toFloat( ) * 10.0 );
}
112 void ZeroToTen::notify( int val ) {
bool b = true;
emit valueChangedf( ( ( float )mapTextToValue( &b ) ) / 10.0 );
}
1 #include <qspinbox.h>
#ifndef CL_CUSTOM_SPINBOX
#define CL_CUSTOM_SPINBOX
7 class ZeroToOne : public QSpinBox {
Q_OBJECT
public:
ZeroToOne( QWidget * parent = 0, const char * name = 0 );
signals:
void valueChangedf( float value );
public slots:
void notify( int val );
void setValuef( float val );
protected:
QString mapValueToText( int value );
int mapTextToValue( bool* ok );
};
28 class ZeroTo500 : public QSpinBox {
Q_OBJECT
public:
ZeroTo500( QWidget * parent = 0, const char * name = 0 );
signals:
void valueChangedf( float value );
public slots:
void notify( int val );
void setValuef( float val );
protected:
QString mapValueToText( int value );
int mapTextToValue( bool* ok );
};
47 class ZeroToTen : public QSpinBox {
Q_OBJECT
public:
ZeroToTen( QWidget * parent = 0, const char * name = 0 );
signals:
void valueChangedf( float value );
public slots:
void notify( int val );
void setValuef( float val );
protected:
QString mapValueToText( int value );
int mapTextToValue( bool* ok );
};
#endif
1 #include <cstdlib>
#include <qdir.h>
#include "Environ.h"
5 bool getResourcePath( QString &result, const QString &file )
{
QDir res_dir;
const char* dir = getenv( RESOURCE_DIR );
if( dir ) {
res_dir.setPath( QString( dir ) );
if( res_dir.exists( ) )
goto got_dir;
}
res_dir.setPath( QString( DATADIR ) + "/resource" );
if ( !res_dir.exists( ) )
return false;
got_dir:
result = res_dir.path( ) + "/" + file;
QFile f( result );
return f.exists( );
}
25 void printResourceError( ) {
std::cerr << "The environment variable " << RESOURCE_DIR
<< " is not defined" << std::endl;
std::cerr << "This contains various essential resources"
"for springy_sim"<< std::endl;
}
1
#ifndef cl_environ
#define cl_environ
#include <qstring.h>
#include <iostream>
/*
* These methods are for dealing with finding the
* location of the resources that nees to be loaded
* at runtime. They all have to be in the directory
* given by the environment variable SPRING_RESOURCE
*/
#define RESOURCE_DIR "SPRING_RESOURCE"
/**
* ATM resources all live in one directory, which is defined at compile time
* ( with configure ). The location of this dir can also be given in an
* environment variable, SPRING_RESOURCE. The env. variable overrides the
* default location, if set.
* This function takes the basename of a \p file and puts the full path to the
* file into \p result, if the file is found in either of these two locations.
*
* \p result The absolute path of the file given ( including the basename )
* \return true on success, false on error
*/
27 bool getResourcePath( QString &result, const QString &file );
/*
* Prints a standard error message.
*/
33 void printResourceError( );
#endif
1 class Error {
public:
3 Error( char* messg );
4 ~Error( );
5 string& message( ) const;
private:
7 string* s;
1 #include "ODESolver.h"
#include <iostream>
4 Euler::Euler( ParticleSystem * p ) :
ODESolver( p ){
temp1 = new float[6];
}
9 Euler::~Euler( ) {
delete[] temp1;
}
13 void Euler::update( float deltaT ) {
//cout << "Euler: entering update" << endl;
Particle *p;
ps->calculateForces( );
for ( unsigned int i = 0; i < ps->size( ); i++ ) {
p = ( *ps )[i];
if ( p->isFixed( ) ) continue;
//cout << "derivative vector" << endl;
// get the "derivative vector" and scale it.
temp1[0] = deltaT * ( p->x[Particle::VEL] );
temp1[1] = deltaT * ( p->x[Particle::VEL+1] );
temp1[2] = deltaT * ( p->x[Particle::VEL+2] );
temp1[3] = deltaT * ( p->x[Particle::FORCE] / p->mass );
temp1[4] = deltaT * ( p->x[Particle::FORCE+1] / p->mass );
temp1[5] = deltaT * ( p->x[Particle::FORCE+2] / p->mass );
//cout << "setting LastX" << endl;
p->x[Particle::LASTX] = p->x[0];
p->x[Particle::LASTX+1] = p->x[1];
p->x[Particle::LASTX+2] = p->x[2];
p->x[0] += temp1[0];
p->x[1] += temp1[1];
p->x[2] += temp1[2];
p->x[Particle::VEL] += temp1[3];
p->x[Particle::VEL+1] += temp1[4];
p->x[Particle::VEL+2] += temp1[5];
}
ps->doCollisions( );
//cout << "Euler: leaving update" << endl;
}
1 #include "Force.h"
5 Const_Field::Const_Field( ParticleSystem* p, float x, float y, float z ){
direction = new float[3];
direction[0] = x;
direction[1] = y;
direction[2] = z;
active = true;
ps = p;
}
15 Const_Field::Const_Field( ParticleSystem* p, QDataStream& qds ){
direction = new float[3];
for( int i=0;i<3;i++ ) qds >> direction[i];
int b; qds >> b;
active = static_cast<bool>( b );
ps = p;
}
24 Const_Field::Const_Field( const Const_Field & f ) {
direction = new float[3];
this->operator=( f );
}
30 Const_Field& Const_Field::operator=( const Const_Field & f ){
if ( this != &f ) {
direction[0] = f.direction[0];
direction[1] = f.direction[1];
direction[2] = f.direction[2];
active = f.active;
}
return *this;
}
40 inline bool Const_Field::isActive( ) const { return active; }
42 inline void Const_Field::activation( bool b ) {
active = b;
}
46 void Const_Field::applyForce( ) const {
if ( !active ) return ;
Particle* p;
for ( unsigned int i = 0; i < ps->size( ); i++ ) {
p = ( *ps )[i];
p->x[Particle::FORCE] += direction[0];
p->x[Particle::FORCE+1] += direction[1];
p->x[Particle::FORCE+2] += direction[2];
}
}
58 QDataStream& Const_Field::operator<<( QDataStream& qds ){
qds << ParticleSystem::CONST_FIELD;
qds << direction[0] << direction[1] << direction[2];
qds << ( int ) active;
return qds;
}
//-----------------------------------------------------------
// Spring methods
//-----------------------------------------------------------
70 Spring::Spring( Particle* first, Particle* second, float springConstant, float dampingFactor,
float restLength ): p1( first ), p2( second ),
ks( springConstant ), kd( dampingFactor ), r( restLength ),
col( 0, 0, 0 ){}
76 Spring::Spring( ParticleSystem* ps, QDataStream& qds ){
int i; qds >> i;
p1 = ps->getParticle( i );
qds >> i;
p2 = ps->getParticle( i );
qds >> ks >> kd >> r >> col;
}
85 Spring::Spring( const Spring& s ) {
this->operator=( s );
}
90 Spring& Spring::operator=( const Spring& s ){
if ( this != &s ) {
p1 = s.p1; p2 = s.p2;
ks = s.ks; kd = s.kd; r = s.r;
}
return *this;
}
99 inline void Spring::setSpringConstant( float val ) {
ks = val;
}
103 inline float Spring::getSpringConstant( ) const {
return ks;
}
107 inline void Spring::setDampingFactor( float val ) {
kd = val;
}
111 inline float Spring::getDampingFactor( ) const {
return kd;
}
115 inline void Spring::setrestLength( float val ) {
r = val;
}
119 inline float Spring::getrestLength( ) const {
return r;
}
124 const QColor& Spring::getColor( ) const {
return col;
}
129 void Spring::setColor( const QColor & c ){
col = c;
}
134 void Spring::applyForce( ) const {
float l[3];
float ldot[3];
float norml;
float dotProd, scalar;
//cout << p1 << p2;
l[0] = p1->x[Particle::POSN] - p2->x[Particle::POSN];
l[1] = p1->x[Particle::POSN+1] - p2->x[Particle::POSN+1];
l[2] = p1->x[Particle::POSN+2] - p2->x[Particle::POSN+2];
ldot[0] = p1->x[Particle::VEL] - p2->x[Particle::VEL];
ldot[1] = p1->x[Particle::VEL+1] - p2->x[Particle::VEL+1];
ldot[2] = p1->x[Particle::VEL+2] - p2->x[Particle::VEL+2];
//cout << "ldot[i]: " << ldot[0] << " " << ldot[1] << endl;
norml = sqrtf( l[0]*l[0] + l[1]*l[1] + l[2]*l[2] );
dotProd = ldot[0]*( l[0] ) + ldot[1]*( l[1] ) + ldot[2]*( l[2] );
scalar = - ( ks*( norml - r ) + kd*( dotProd / norml ) ) / norml;
//cout << "norml : " << norml << endl;
//cout << "dotProd : " << dotProd << endl;
//cout << "scalar: " << scalar << endl;
//cout << "l[i] : " << l[0] << " " <<l[1] << endl << endl;
p1->x[Particle::FORCE] += scalar * l[0];
p2->x[Particle::FORCE] -= scalar * l[0];
p1->x[Particle::FORCE+1] += scalar * l[1];
p2->x[Particle::FORCE+1] -= scalar * l[1];
p1->x[Particle::FORCE+2] += scalar * l[2];
p2->x[Particle::FORCE+2] -= scalar * l[2];
//cout << p1 << p2;
}
168 bool Spring::usesParticle( const Particle* p ) const {
if ( p == p1 || p == p2 ) return true;
else return false;
}
175 QDataStream& Spring::operator<<( QDataStream& qds ){
qds << ParticleSystem::SPRING;
qds << p1->id( ) << p2->id( );
qds << ks << kd << r << col;
return qds;
}
//-----------------------------------------------------------
// Global_Drag methods
//-----------------------------------------------------------
187 Global_Drag::Global_Drag( ParticleSystem* p, float coeffOfDrag ) :
coeff( coeffOfDrag ), ps( p ) {}
191 Global_Drag::Global_Drag( ParticleSystem* p, QDataStream& qds ){
qds >> coeff;
ps = p;
}
197 void Global_Drag::applyForce( ) const {
Particle* p;
for ( unsigned int i = 0; i < ps->size( ); i++ ) {
p = ( *ps )[i];
p->x[Particle::FORCE] -= coeff*( p->x[Particle::VEL] );
p->x[Particle::FORCE+1] -= coeff*( p->x[Particle::VEL+1] );
p->x[Particle::FORCE+2] -= coeff*( p->x[Particle::VEL+2] );
}
}
208 QDataStream& Global_Drag::operator<<( QDataStream& qds ){
qds << ParticleSystem::GLOBAL_DRAG;
qds << coeff;
return qds;
}
//-----------------------------------------------------------
// Drag methods
//-----------------------------------------------------------
219 Drag::Drag( Particle* target, float coeffOfDrag ) :
p( target ), coeff( coeffOfDrag ) {}
223 Drag::Drag( ParticleSystem* ps, QDataStream& qds ){
int i; qds >> i;
p = ps->getParticle( i );
qds >> coeff;
}
230 inline float Drag::getCoeff( ) const {
return coeff;
}
235 inline void Drag::setCoeff( float val ) {
coeff = val;
}
240 void Drag::applyForce( ) const {
p->x[Particle::FORCE] -= coeff * ( p->x[Particle::VEL] );
p->x[Particle::FORCE+1] -= coeff * ( p->x[Particle::VEL+1] );
p->x[Particle::FORCE+2] -= coeff * ( p->x[Particle::VEL+2] );
}
247 bool Drag::usesParticle( const Particle* pc ) const {
if ( pc == p ) return true;
else return false;
}
253 QDataStream& Drag::operator<<( QDataStream& qds ){
qds << ParticleSystem::DRAG;
qds << p->id( );
qds << coeff;
return qds;
}
//-----------------------------------------------------------
// Constant methods
//-----------------------------------------------------------
265 Constant::Constant( Particle* target, float xval,
float yval, float zval ) :
p( target ) {
f[0] = xval;
f[1] = yval;
f[2] = zval;
}
273 Constant::Constant( ParticleSystem* ps, QDataStream& qds ){
int i; qds >> i;
p = ps->getParticle( i );
for( i=0;i<3;i++ ) qds>>f[i];
}
280 inline float Constant::getVal( int whichval ) const {
if( whichval < 0 || whichval > 2 ) return 0.0;
return f[whichval];
}
286 inline void Constant::setVal( float newval, int whichval ) {
if( whichval < 0 || whichval > 2 ) return;
f[whichval] = newval;
}
292 void Constant::applyForce( ) const {
p->x[Particle::FORCE] += f[0];
p->x[Particle::FORCE+1] += f[1];
p->x[Particle::FORCE+2] += f[2];
}
300 bool Constant::usesParticle( const Particle* pc ) const {
if ( pc == p ) return true;
else return false;
}
306 QDataStream& Constant::operator<<( QDataStream& qds ){
qds << ParticleSystem::CONSTANT;
qds << p->id( );
for( int i =0;i < 3;i++ ) qds << f[i];
return qds;
}
315 QDataStream& operator<<( QDataStream& qds, Force& f ){
f.operator<<( qds );
return qds;
}
1
#include <math.h>
#include <qcolor.h>
#include <qdatastream.h>
#ifndef cl_Force
#define cl_Force
9 class Particle;
10 class ParticleSystem;
12 class Force {
public:
15 Force( ){}
16 virtual ~Force( ){}
17 virtual void applyForce( ) const {}
//overide this function if the Force contains
// a pointer or reference to a particle.
20 virtual bool usesParticle( const Particle* ) const {
return false;
}
23 virtual QDataStream& operator<<( QDataStream& qds ){
return qds;
}
};
29 class Const_Field: public Force {
30 ParticleSystem *ps;
float* direction;
32 bool active;
public:
35 Const_Field( ParticleSystem* p, float x = 0.0,
float y = 1.0, float z = 0.0 );
//build object from file.
38 Const_Field( ParticleSystem* p, QDataStream& qds );
40 Const_Field( const Const_Field & f );
41 Const_Field& operator=( const Const_Field & f );
42 ~Const_Field( ) { delete[] direction; }
43 bool isActive( ) const;
44 void activation( bool b );
45 virtual void applyForce( ) const;
46 inline void setY( float val ) { direction[1] = val; }
47 inline float getY( ) const { return direction[1]; }
48 virtual QDataStream& operator<<( QDataStream& qds );
};
53 class Spring : public Force {
protected:
56 Particle* p1;
57 Particle* p2;
float ks; // spring constant
float kd; // damping constant
float r; // rest length;
61 QColor col;
62 Spring( ){} // for subclasses
public:
65 Spring( Particle* first, Particle* second,
float springConstant = 0.4, float dampingFactor = 0.2,
float restLength = 100.0 );
68 Spring( ParticleSystem*, QDataStream& qds );
69 Spring( const Spring& s );
70 Spring& operator=( const Spring& s );
71 ~Spring( ){}
73 void setSpringConstant( float val );
74 float getSpringConstant( ) const ;
75 void setDampingFactor( float val );
76 float getDampingFactor( ) const;
77 void setrestLength( float val );
78 float getrestLength( )const ;
79 const Particle* getParticle1( ) const{ return p1; }
80 const Particle* getParticle2( ) const { return p2; }
81 virtual void applyForce( ) const;
83 const QColor& getColor( ) const ;
84 void setColor( const QColor & );
85 virtual bool usesParticle( const Particle* ) const;
86 virtual QDataStream& operator<<( QDataStream& qds );
};
91 class Global_Drag : public Force {
float coeff;
94 ParticleSystem* ps;
public:
97 Global_Drag( ParticleSystem* p, float coeffOfDrag = 0.08 );
98 Global_Drag( ParticleSystem*, QDataStream& qds );
99 inline float getCoeff( ) const { return coeff; }
100 inline void setCoeff( float val ) { coeff = val; }
101 virtual void applyForce( ) const ;
102 virtual QDataStream& operator<<( QDataStream& qds );
};
106 class Drag : public Force {
108 Particle* p;
float coeff;
public:
112 Drag( Particle* target, float coeffOfDrag = 0.02 );
113 Drag( ParticleSystem*, QDataStream& qds );
114 float getCoeff( ) const;
115 void setCoeff( float val );
116 virtual void applyForce( ) const;
117 virtual bool usesParticle( const Particle* ) const;
118 virtual QDataStream& operator<<( QDataStream& qds );
};
122 class Constant : public Force {
124 Particle* p;
float f[3];
public:
128 Constant( Particle* target, float xval = 2.0f,
float yval = 0.0f, float zval = 0.0f );
130 Constant( ParticleSystem*, QDataStream& qds );
131 float getVal( int whichval ) const;
132 void setVal( float newval, int whichval );
133 virtual void applyForce( ) const;
134 virtual bool usesParticle( const Particle* ) const;
135 virtual QDataStream& operator<<( QDataStream& qds );
};
//global
140 QDataStream& operator<<( QDataStream& qds, Force& f );
#include "Particle.h"
#endif //cl_Force
1 #include "Particle.h"
#include "Force.h"
#include "Constraint.h"
#ifndef CL_LOADSAVE
#define CL_LOADSAVE
9 class SysSaver {
};
14 class SysLoader {
};
1 #include "ODESolver.h"
#include <iostream>
4 MidPoint::MidPoint( ParticleSystem * p ) :
ODESolver( p ){
if( ( num_pcls = ps->size( ) ) > 0 )
yn = new float[3*num_pcls];
else
yn = 0;
yn_size = num_pcls;
}
15 MidPoint::~MidPoint( ) {
delete[] yn;
}
19 void MidPoint::update( float deltaT ) {
//cout << "MidPoint: entering update" << endl;
// make sure yn is big enough
if( ( num_pcls = ps->size( ) ) == 0 ) return;
if ( num_pcls > yn_size ) {
yn_size = num_pcls;
delete [] yn;
yn = new float[3*yn_size];
}
// save the original velocity
// values in yn
Particle *p;
unsigned int j, k; j = 0;
for( unsigned int i = 0; i < num_pcls; i++ ) {
p = ( *ps )[i];
for( k = 0; k < 3; k++ )
yn[j+k] = p->x[Particle::VEL+k];
j+=3;
//keep the previous position here
for( k = 0; k < 3; k++ )
p->x[Particle::LASTX+k] = p->x[k];
}
// do the first Euler bit to move the sim
// to the midpoint.
halfDel = deltaT / 2.0;
ps->calculateForces( );
for ( unsigned int i = 0; i < ps->size( ); i++ ) {
p = ( *ps )[i];
if ( p->isFixed( ) ) continue;
//cout << "derivative vector" << endl;
// get the "derivative vector" and scale it.
for ( int j = 0 ; j < 3 ;j++ )
p->x[Particle::POSN+j] +=
( halfDel*( p->x[Particle::VEL+j] ) );
for ( int j = 0 ; j < 3 ;j++ )
p->x[Particle::VEL+j] +=
( halfDel * ( ( p->x[Particle::FORCE+j] )/ p->mass ) );
}
ps->doCollisions( );
ps->calculateForces( );
for ( unsigned int i = 0; i < ps->size( ); i++ ) {
p = ( *ps )[i];
for ( int j = 0 ; j < 3 ;j++ )
p->x[Particle::POSN+j] =
( p->x[Particle::LASTX+j] + ( deltaT*( p->x[Particle::VEL+j] ) ) );
for ( int j = 0 ; j < 3 ;j++ )
p->x[Particle::VEL+j] =
( yn[( i*3 )+j] ) + ( deltaT * ( ( p->x[Particle::FORCE+j] )/ p->mass ) );
}
ps->doCollisions( );
//cout << "MidPoint: leaving update" << endl;
}
1 #include "Mouse_Spring.h"
3 Mouse_Spring::Mouse_Spring( ParticleSystem *psys ) {
p1 = 0;
ks = 2.2; kd = 3.0;
ps = psys;
active = false;
x = y = 0;
}
12 Mouse_Spring::~Mouse_Spring( ) {}
14 bool Mouse_Spring::isActive( ) const {
return active;
}
19 void Mouse_Spring::pickParticle( int mousex, int mousey ) {
//cout << "Mouse_Spring::pick_Particle( )" << endl;
p1 = 0;
Particle* p;
float min = 200000.0; float prog;
for ( unsigned int i = 0; i < ps->size( ); i++ ) {
p = ( *ps )[i];
prog = pow( ( p->x[0] - ( float )mousex ), 2 ) +
pow( ( p->x[1] - ( float )mousey ), 2 );
if ( prog < min ) {
p1 = p; min = prog;
}
}
if( !p1 ) active = false;
else active = true;
}
37 void Mouse_Spring::update_posn( int mousex, int mousey ){
x = ( float ) mousex;
y = ( float ) mousey;
//cout << "Mouse_Spring::update_posn( )" <<
// x << " " << y <<endl;
}
46 void Mouse_Spring::releaseParticle( ) {
active = false;
}
50 void Mouse_Spring::applyForce( ) const
{
if ( !active )
return;
float l[2];
// std::cout << "Mouse_Spring::applyForce( ) " << x << " "<< y << std::endl;
float norml;
float dotProd, scalar;
static int v = Particle::VEL;
//cout << p1 << p2;
l[0] = p1->x[Particle::POSN] - x;
l[1] = p1->x[Particle::POSN+1] - y;
norml = sqrtf( l[0]*l[0] + l[1]*l[1] );
// 2D only
dotProd = p1->x[v]*( l[0] ) + p1->x[v+1]*( l[1] );
scalar = - ( ks*( norml - r ) + kd*( dotProd / norml ) ) / norml;
//cout << "norml : " << norml << endl;
//cout << "dotProd : " << dotProd << endl;
//cout << "scalar: " << scalar << endl;
//cout << "l[i] : " << l[0] << " " <<l[1] << endl << endl;
p1->x[Particle::FORCE] += scalar * l[0];
p1->x[Particle::FORCE+1] += scalar * l[1];
// std::cout << p1 ;
}
1 #ifndef cl_m_spring
#define cl_m_spring
#include <iostream>
#include "Force.h"
#include "Particle.h"
// a fairly dodgy piece of inheritance
// don't call the Spring::getParticle( ) routines
// on a Mouse_Spring.
// Mouse_Spring alocates memory for one of its 'particles'
// which represets the pointer when solving the spring equation
// hence the non-empty destructor.
14 class Mouse_Spring: public Force {
15 ParticleSystem* ps;
16 bool active;
static const float r = 0;
float ks; float kd;
float x, y;
20 Particle* p1;
public:
23 Mouse_Spring( ParticleSystem * psys );
24 ~Mouse_Spring( );
25 bool isActive( ) const;
26 virtual void applyForce( ) const;
27 void pickParticle( int mousex, int mousey );
28 void update_posn( int mousex, int mousey );
29 void releaseParticle( );
30 inline Particle* getTarget( ) const { return p1; }
};
#endif
1
#ifndef cl_SOLVER
#define cl_SOLVER
#include "Particle.h"
7 class ODESolver {
public:
10 ODESolver( ParticleSystem * sys ) :
ps( sys ){}
12 virtual ~ODESolver( ){}
13 virtual void update( float deltaT ) = 0;
protected:
16 ParticleSystem *ps;
};
20 class Euler : public ODESolver {
public:
23 Euler( ParticleSystem * sys );
24 virtual ~Euler( );
25 virtual void update( float deltaT );
private:
float *temp1;
};
32 class MidPoint : public ODESolver {
public:
35 MidPoint( ParticleSystem * sys );
36 virtual ~MidPoint( );
37 virtual void update( float deltaT );
private:
float* yn;
unsigned int num_pcls, yn_size;
float halfDel;
};
46 class RungeKutta4 : public ODESolver {
public:
49 RungeKutta4( ParticleSystem * sys );
50 virtual ~RungeKutta4( );
51 virtual void update( float deltaT );
private:
float halfDel, sixth;
unsigned int num_pcls, yn_size;
float *result, *yn;
static const float factor[];
};
#endif
1 #include <vector>
#include "Particle.h"
#include <iostream>
#include <math.h>
using namespace std;
//-----------------------------------------------------
// Particle methods
//-----------------------------------------------------
13 Particle::Particle( int id ) {
idnum = id;
x = new float[ARRAY_SIZE]; // save a few calls to new.
mass = 1.0;
for( int i = 0; i < 12; i++ ) x[i] = 0.0;
//default colour is white.
for( int i = 12; i < 15; i++ ) x[i] = 1.0;
fixed = false;
}
24 Particle::Particle( Particle& p ) {
x = new float[ARRAY_SIZE];
this->operator=( p );
}
29 Particle& Particle::operator=( Particle& p ) {
if ( this == &p ) return *this;
for ( unsigned int i = 0; i < ARRAY_SIZE; i++ ) x[i] = p.x[i];
mass = p.mass;
fixed = p.fixed;
return *this;
}
38 Particle::~Particle( ) {
delete[] x;
}
42 void Particle::setFixed( bool b ){
fixed = b;
}
46 bool Particle::isFixed( ) const{
return fixed;
}
50 void Particle::pickColor( ) {
int i, j;
float f, rounded;
for( int q = 0; q < 3 ; q++ ) {
i = rand( );
j = rand( );
f = ( ( float ) i ) / ( ( float ) j );
rounded = floor( f );
f = f - rounded;
//f = abs( f );
x[COLOR+q] = f + 0.3;
}
}
65 unsigned int Particle::id( ) const {
return idnum;
}
69 void Particle::id( unsigned int i ){
idnum = i;
}
//-----------------------------------------------------
// Global IO functions
//-----------------------------------------------------
79 ostream& operator<< ( ostream & os, const Particle* p ) {
for ( unsigned int i = 0; i < Particle::ARRAY_SIZE; i++ ) {
cout << p->x[i] << " ";
if ( ( i+1 )%3 == 0 ) cout << "| ";
}
cout << endl;
return os;
}
89 QDataStream& operator<< ( QDataStream& qds, const Particle& p ){
for ( unsigned int i = 0; i < Particle::ARRAY_SIZE; i++ ) {
qds << p.x[i];
}
qds << p.idnum;
qds << ( int ) p.fixed;
qds << p.mass;
return qds;
}
100 QDataStream& operator>> ( QDataStream& qds, Particle& p ){
for ( unsigned int i = 0; i < Particle::ARRAY_SIZE; i++ ) {
qds >> p.x[i];
}
int i;
qds >> p.idnum;
qds >> i;
p.fixed = static_cast<bool>( i );
qds >> p.mass;
return qds;
}
113 QDataStream& operator<<( QDataStream & qds,
const ParticleSystem::SysElement& e ){
qds <<( int ) e;
return qds;
}
120 QDataStream& operator>>( QDataStream & qds,
ParticleSystem::SysElement& e ){
int i;
qds >> i;
e = static_cast<ParticleSystem::SysElement>( i );
return qds;
}
129 QDataStream& operator<<( QDataStream & qds,
const ParticleSystem& ps ){
QString name = "ParticleSystem Filev0.1";
qds << name;
qds << ps.particles->size( );
//write out the particles
for ( unsigned int i=0; i < ps.particles->size( );i++ )
qds << *( ps.getParticle( i ) );
for ( unsigned int j=0; j < ps.forces->size( );j++ )
qds << *( ( *ps.forces )[j] );
qds << ParticleSystem::END;
if ( ps.cs )
if( ps.cs->size( ) > 0 ) qds << *( ps.cs );
return qds;
}
//-----------------------------------------------------
// ParticleSystem Methods
//-----------------------------------------------------
153 ParticleSystem::ParticleSystem( int numParticles ) {
particles = new vector<Particle*>;
forces = new vector<Force*>;
colliders = new vector<D2_Collider*>;
for ( int i = 0; i < numParticles; i++ ) {
particles->push_back( new Particle( i ) );
}
changed = true;
cs= 0;
}
164 ParticleSystem::~ParticleSystem( ) {
//delete particles;
for ( unsigned int i = 0; i < particles->size( ); i++ )
delete ( *particles )[i];
delete particles;
//delete all the forces
for ( unsigned int i = 0; i < forces->size( ); i++ )
delete ( *forces )[i];
delete forces;
//delete Colliders
for ( unsigned int i = 0; i < colliders->size( ); i++ )
delete ( *colliders )[i];
delete colliders;
delete cs;
}
183 unsigned int ParticleSystem::size( ) {
return particles->size( );
}
187 const vector<Particle*>* ParticleSystem::getSystem( ) const {
return particles;
}
192 void ParticleSystem::addForce( Force* f ) {
forces->push_back( f );
}
197 void ParticleSystem::addCollider( D2_Collider* c ) {
colliders->push_back( c );
}
203 Particle* ParticleSystem::addParticle( float x, float y, float z ) {
Particle *p = new Particle( particles->size( ) );
p->x[Particle::POSN] = p->x[Particle::LASTX] = x;
p->x[Particle::POSN+1] = p->x[Particle::LASTX+1] = y;
p->x[Particle::POSN+2] = p->x[Particle::LASTX+2] = z;
particles->push_back( p );
changed = true;
return p;
}
216 Particle* ParticleSystem::getParticle( int index ) const {
return ( *particles )[index];
}
221 Particle* ParticleSystem::operator[]( int index ) const {
return ( *particles )[index];
}
225 void ParticleSystem::clear( ) {
Particle* p;
while( !particles->empty( ) ) {
p = particles->back( );
particles->pop_back( );
delete p;
p = 0;
}
clearSprings( );
changed = true;
}
237 void ParticleSystem::clearSprings( ) {
Spring* s = 0;
vector<Force*>::iterator i = forces->begin( );
while( i != forces->end( ) ) {
s = dynamic_cast<Spring*>( *i );
if( s ) { i = forces->erase( i ); delete s; }
else i++;
s = 0;
}
}
250 int ParticleSystem::numForces( ) const{
return forces->size( );
}
254 const Force* ParticleSystem::getForce( int index ) const {
return ( *forces )[index];
}
258 void ParticleSystem::calculateForces( ) {
//first zero forces
Particle* p;
for ( unsigned int i = 0; i < particles->size( ); i++ ) {
p = ( *particles )[i];
p->x[Particle::FORCE] = 0;
p->x[Particle::FORCE+1] = 0;
p->x[Particle::FORCE+2] = 0;
}
for( unsigned int i = 0; i < forces->size( ); i++ )
( *forces )[i]->applyForce( );
if( cs ) cs->adjustForces( );
}
276 void ParticleSystem::doCollisions( ) {
if ( cs ) return;
for ( unsigned int i = 0; i < colliders->size( );i++ )
( *colliders )[i]->solveCollisions( );
}
285 void ParticleSystem::renumber( ) {
for( unsigned int i = 0; i < particles->size( );i++ )
( ( *particles )[i] )->id( i );
}
291 ConstraintSystem* ParticleSystem::enableConstraints( bool b ) {
if( b ) {
delete cs;
cs = new ConstraintSystem( *this );
return cs;
}
delete cs; cs = 0; return 0;
}
302 ConstraintSystem* ParticleSystem::getCS( ) {
return cs;
}
307 void ParticleSystem::removeParticle( Particle* p ) {
//make sure the particle exists
P_Iter j = particles->begin( );
bool found = false;
while ( j != particles->end( ) ) {
if( *j == p ) {
found = true;
break;
}
else j++;
}
if( !found ) { //bum pointer passed in
cout << "Bad pointer passed to "<<
"ParticleSystem::removeParticle( )"<< endl;
return;
}
// get rid of any Constraints that refer to
// the particle
if( cs ) cs->removeParticleRefs( p );
// get rid of forces likewise
F_Iter i = forces->begin( );
while( i != forces->end( ) ) {
if( ( *i )->usesParticle( p ) ){
delete *i;
i = forces->erase( i );
}
else i++;
}
//now actually remove the particle
delete *j;
particles->erase( j );
//renumber particles
j = particles->begin( ); int k = 0;
while ( j != particles->end( ) ) {
( *( j++ ) )->id( k++ );
}
changed = true;
}
352 bool ParticleSystem::isChanged( ) {
bool ret = changed;
changed = false;
return ret;
}
// used in loading from file
359 void ParticleSystem::clearAllForces( ) {
F_Iter i = forces->begin( );
while( i != forces->end( ) ) {
delete *i;
i = forces->erase( i );
}
}
367 void ParticleSystem::load( QDataStream& qds,
368 Global_Drag*& d, Const_Field*& f ) {
clear( ); clearAllForces( );
QString s;
qds >> s;
if( s != "ParticleSystem Filev0.1" ){
cout << "ParticleSystem incorrect file format!!"<< endl;
//*d = 0; *f=0;
cout << s << endl;
return;
}
// make particles.
int nump;
qds >> nump;
Particle*p;
for( int i=0;i < nump;i++ ){
p = new Particle( );
qds >> *p;
particles->push_back( p );
}
// make forces
SysElement e; bool exit = false;
Force* pf = 0;
while( !exit ) {
qds >> e;
switch( e ) {
case END :
exit = true; pf = 0;
break;
case CONST_FIELD :
f = new Const_Field( this, qds );
pf = f;
break;
case SPRING :
pf = new Spring( this, qds );
break;
case GLOBAL_DRAG :
d = new Global_Drag( this, qds );
pf = d;
break;
case DRAG :
pf = new Drag( this, qds );
break;
case CONSTANT:
pf = new Constant( this, qds );
break;
default:
cout<< "ParticleSystem::load( )"<<
"fallen thru case!!"<<endl;
}
if( pf ) forces->push_back( pf );
}
//make constraints ( if any )
if( qds.atEnd( ) ) {
enableConstraints( false );
return;
}
//make the constraints
qds >> e;
if( e != CONSTRAINTSYSTEM )
cout << "ParticleSystem::load( ) : file format error2"<<
endl;
if( !cs )
enableConstraints( true );
cs->loadFromFile( qds );
changed = true;
}
1 #include <vector>
#include <iostream>
#include <qdatastream.h>
#include <qstring.h>
#ifndef cl_particle
#define cl_particle
#include "Constraint.h"
#include "Force.h"
#include "Plane.h"
12 class Particle {
public:
15 Particle( int id = 0 );
16 Particle( Particle& p );
17 Particle& operator=( Particle& p );
18 ~Particle( );
float * x; // all the stuff's in here
float mass;
22 void pickColor( );
23 void setFixed( bool b );
24 bool isFixed( ) const;
25 unsigned int id( ) const;
26 void id( unsigned int i );
28 friend std::ostream& operator<<( std::ostream& os, const Particle* p );
29 friend QDataStream& operator<<( QDataStream& qds, const Particle& p );
30 friend QDataStream& operator>>( QDataStream& qds, Particle& p );
static const unsigned int POSN = 0;
static const unsigned int VEL = 3;
static const unsigned int FORCE = 6;
static const unsigned int LASTX = 9;
static const unsigned int COLOR = 12;
private:
static const unsigned int ARRAY_SIZE = 15;
39 bool fixed;
unsigned int idnum;
};
#ifndef cl_Force
48 class Force;
49 extern void Force::applyForce( ) const;
#endif
52 class ConstraintSystem;
53 class D2_Collider;
54 class ParticleSystem {
public:
57 ParticleSystem( int numParticles = 0 );
58 ~ParticleSystem( );
59 unsigned int size( );
60 const std::vector<Particle*>* getSystem( ) const;
61 void addForce( Force* f );
// Particle System manages memory
// for the Colliders placed in it!!!
65 void addCollider( D2_Collider* c );
66 void calculateForces( );
67 void doCollisions( );
// create a particle with given position
// returns a pointer to the new particle
71 Particle* addParticle( float x, float y, float z );
72 Particle* getParticle ( int index )const;
73 void clear( );
74 void clearSprings( );
// returns true if particles have been added or
// removed from the system since the last time
// this function was called. Adding/removing springs
// or forces has no effect.
80 bool isChanged( );
81 Particle* operator[]( int index ) const;
82 int numForces( ) const;
83 const Force* getForce( int index ) const;
// If called with true, returns a pointer
// to the new ConstraintSystem so that
// Constraints can be added directly.
// don't delete the pointer.
// returns 0 if called with false
// any previously returned pointers will
// be invalid.
93 ConstraintSystem* enableConstraints( bool b );
//returns a pointer to the existing CS if one
// is enabled, otherwise returns 0;
97 ConstraintSystem* getCS( );
99 void removeParticle( Particle* p );
enum SysElement{
PARTICLE, FORCE, CONST_FIELD, SPRING,
GLOBAL_DRAG, DRAG, CONSTANT, CONSTRAINTSYSTEM,
COLLIDER, D2_PLANE, END};
106 friend QDataStream& operator<<( QDataStream &,
const ParticleSystem& );
// calling this will invaidate ALL pointers
// which point to stuff owned by the ParticleSystem
// everything is cleared BEFORE loading from the
// datastream apart from colliders at present
113 void load( QDataStream& qds, Global_Drag*&, Const_Field*& );
private:
116 std::vector<Particle* > * particles;
117 std::vector<Force* > * forces;
118 std::vector<D2_Collider* > * colliders;
119 ConstraintSystem* cs;
// used to reset the id number of all
// the particles if one is deleted. These
// numbers are used by the constraint system
123 void renumber( );
124 bool changed;
125 void clearAllForces( );
typedef std::vector<Force* >::iterator F_Iter;
typedef std::vector<Particle*>::iterator P_Iter;
};
//global io
132 QDataStream& operator<<( QDataStream &,
const ParticleSystem::SysElement& );
134 QDataStream& operator>>( QDataStream &,
ParticleSystem::SysElement& );
#endif
1
#include "ParticleField.h"
#include <qpainter.h>
#include <math.h>
#include <qapplication.h>
#include <qcolor.h>
#include <matrix/Matrix.h>
10 ParticleField::ParticleField( QWidget *parent, const char *name )
: QWidget( parent, name )
{
setPalette( QPalette( QColor( 250, 250, 200 ) ) );
ps = new ParticleSystem( );
cs = ps->enableConstraints( true );
gravity = new Const_Field( ps, 0.0, 35.0, 0.0 );
ps->addForce( gravity );
ms = new Mouse_Spring( ps ); //don't delete!!
ps->addForce( ms );
drag = new Global_Drag( ps );
ps->addForce( drag );
e = new Euler( ps );
st = EULER;
clicked = 0;
deltaT = 0.06;
interval = 30;
leftButtonPressed = false;
sprType = 0;
running = false;
timer = new QTimer( this );
connect( timer, SIGNAL( timeout( ) ),
this, SLOT( update( ) ) );
setFocusPolicy( QWidget::StrongFocus );
bg = new QColor( 220, 180, 240 );
bgBrush = new QBrush( *bg );
// set up modes
mode = CONSTRAINTS; mousemode = SELECT;
// Keeps track of selected particles
selected = new std::set<Particle*>( );
// no tracing initially
tracer = 0;
tracing = false; drawTrace = false;
tracePen.setRgb( 40, 20, 20 );
//no grid
snapToGrid = false; displayGrid = false;
gridSize = 50;
buildBorders( );
setFixedSize( 600, 600 );
setBackgroundMode( NoBackground );
drawComplexSpring = true;
}
66 ParticleField::~ParticleField( ) {
delete ps;
delete e;
delete bg;
//delete bgBrush;
}
73 void ParticleField::mousePressEvent ( QMouseEvent * me ) {
thisx = me->x( ); thisy = me->y( );
// nothing happens on right mouse press
if ( me->button( ) == RightButton ) return;
leftButtonPressed = true;
if( me->state( ) & ControlButton ) {
//grap the particle
ms->pickParticle( me->x( ), me->y( ) );
}
else {
if( mousemode == MAKESPRING ) {
// going to build a spring
Particle* p = selectParticle( me );
if ( p == 0 ) return;
clicked = p;
}
else {
//record corner of the selection
// rectangle
startx = me->x( );
starty = me->y( );
}
}
}
104 void ParticleField::mouseReleaseEvent ( QMouseEvent * me ) {
// RIGHT BUTTON
if ( me->button( ) == RightButton ) {
// create a new Particle.
addParticle( me->x( ), me->y( ),
( bool ) ( me->state( ) & ShiftButton ) );
}
// LEFT BUTTON
else if ( me->button( ) == LeftButton ) {
if( mousemode == MAKESPRING ) {
makeSpring( me );
}
else {
// select all the particles that are
// in the box
if( !( me->state( ) & ShiftButton ) ) selected->clear( );
//because we don't really
if( !ms->isActive( ) )
boxSelect( me );
}
leftButtonPressed = false;
clicked = 0; // kill partial spring
ms->releaseParticle( ); //kill the mousespring
}
// repaint window ( BOTH BUTTONS )
QPaintEvent* pev = new QPaintEvent( rect( ) );
paintEvent( pev );
delete pev;
}
138 void ParticleField::paintEvent( QPaintEvent * ) {
QPainter painter( &pix );
//draw the background.
if( drawTrace ) {
QPoint p( 0, 0 );
bitBlt( &pix, p, &tracemap, rect( ), CopyROP );
}
else
painter.fillRect( rect( ), *bgBrush );
if( displayGrid ) drawGrid( painter );
drawSprings( painter );
drawConstraints( painter );
drawParticles( painter );
drawMouseArt( painter );
painter.end( );
painter.begin( this );
painter.drawPixmap( rect( ).topLeft( ), pix );
}
160 void ParticleField::mouseMoveEvent( QMouseEvent *me )
// if the sim is running, we just have to note
// the current mouse pos.
// if the sim is stopped, we still have to do these things
// but have to generate a paintevent as well.
{
thisx = me->x( ); thisy = me->y( );
if ( leftButtonPressed ) {
// also tell the mousespring about it
if ( ms->isActive( ) ) ms->update_posn( me->x( ), me->y( ) );
// generate paintEvent
if( !running ) {
QPaintEvent * pe = new QPaintEvent( this->rect( ) );
paintEvent( pe );
delete pe;
}
}
}
183 void ParticleField::keyPressEvent ( QKeyEvent * ke ) {
int val = ke->key( );
switch( val ){
case Key_G : runSim( !running );// start if stopped
break; // and vice versa
case Key_S : if ( running ) runSim( false ); //stop
else update( ); //just do one step
break;
case Key_Delete : deleteSelectedParticles( );
break;
default: ke->ignore( );
}
}
/*
QSizePolicy ParticleField::sizePolicy( ) const
{
return QSizePolicy( QSizePolicy::Expanding, QSizePolicy::Expanding );
} */
206 Particle* ParticleField::selectParticle( QMouseEvent * me ) {
Particle* p = 0;
Particle* r = 0;
for ( unsigned int i = 0; i < ps->size( ); i++ ) {
p = ( *ps )[i];
if ( pow( ( p->x[0] - ( float )me->x( ) ), 2 ) +
pow( ( p->x[1] - ( float )me->y( ) ), 2 ) < 100.0 ) {
r = p; break;
}
}
return r;
}
220 void ParticleField::drawParticles( QPainter & painter ) {
Particle* p;
//std::cout << selected->size( ) << std::endl;
painter.setPen( NoPen );
for ( unsigned int i = 0; i < ps->size( ); i++ ) {
p = ( *ps )[i];
// draw a highlight on selected particles
if( selected->find( p ) != selected->end( ) ) {
painter.setBrush( white );
painter.drawPie( ( int )p->x[0] - 7, ( int )p->x[1] - 7,
14, 14, 0, 5760 );
}
if ( p->isFixed( ) ) painter.setBrush( red );
else painter.setBrush( blue );
painter.drawPie( ( int )p->x[0] - 5, ( int )p->x[1] - 5,
10, 10, 0, 5760 );
}
}
244 void ParticleField::drawSprings( QPainter& painter ) {
painter.setPen( Qt::SolidLine ) ;
const Force* f = 0;
const Spring* s = 0;
const Particle* p1;const Particle* p2;
int max = ps->numForces( );
for ( int i = 0; i < max ;i++ ) {
f = ps->getForce( i );
s = dynamic_cast<const Spring*>( f );
if( !s ) continue;
p1 = s->getParticle1( );
p2 = s->getParticle2( );
painter.setPen( s->getColor( ) );
if( drawComplexSpring )
drawSpringMatt( painter, p1->x[0], p1->x[1], p2->x[0],
p2->x[1] );
else
painter.drawLine( ( int ) p1->x[0], ( int ) p1->x[1],
( int ) p2->x[0], ( int ) p2->x[1] );
s = 0;
}
}
//this method written by Matthew Raffety, coded by Carl Lewis
// variables: alen : length of the small lead from each point to spring
// S_H : leght of hypotenuse of triangle
void
271 ParticleField::drawSpringMatt( QPainter& paint, float pZeroX , float pZeroY,
float pOneX, float pOneY ) {
float alen = 20; float nk = 40;
float deltaX = pOneX - pZeroX;
float deltaY = pOneY - pZeroY;
float deltaLen = sqrtf( deltaX*deltaX + deltaY*deltaY );
if ( deltaLen < 2 * alen ) {
paint.drawLine( ( int ) pZeroX, ( int ) pZeroY, ( int ) pOneX, ( int ) pOneY );
return;
}
float bigAX = alen * deltaX / deltaLen;
float bigAY = alen * deltaY / deltaLen;
float width = deltaLen - 2.0*alen;
float littleS = 2.0* width / nk; // nk = number of kinks
float S_H = 30.0 ; //should probably be a parameter
if( littleS >= S_H ) {
paint.drawLine( ( int ) pZeroX, ( int ) pZeroY, ( int ) pOneX, ( int ) pOneY );
return;
}
float vectorSX = ( deltaX - 2*bigAX ) / nk;
float vectorSY = ( deltaY - 2*bigAY ) / nk;
float height = sqrtf( S_H*S_H - littleS*littleS );
float bigMX = height* deltaY / deltaLen;
float bigMY = -height * deltaX / deltaLen;
float DrawX = pZeroX + bigAX + ( 0.5 * vectorSX );
float DrawY = pZeroY + bigAY + ( 0.5 * vectorSX );
paint.moveTo( ( int ) pZeroX, ( int ) pZeroY );
paint.lineTo( ( int ) ( pZeroX + bigAX ) , ( int ) ( pZeroY + bigAY ) );
for( int i = 0; i < nk; i++ ) {
if( i & 0x1 ) paint.lineTo( ( int )( DrawX + bigMX ) , ( int )( DrawY + bigMY ) );
else paint.lineTo( ( int )( DrawX - bigMX ) , ( int )( DrawY - bigMY ) );
DrawX += vectorSX; DrawY += vectorSY;
}
paint.lineTo( ( int ) ( pOneX - bigAX ), ( int )( pOneY - bigAY ) );
paint.lineTo( ( int ) pOneX , ( int ) pOneY ) ;
}
320 void ParticleField::drawBox( QPainter& painter ) {
if( mousemode != SELECT || leftButtonPressed == false ) return ;
painter.setBrush( NoBrush );
painter.setPen( Qt::SolidLine );
painter.drawRect( startx, starty,
( thisx- startx ), ( thisy - starty ) );
}
329 void ParticleField::drawMouseArt( QPainter & painter ) {
// This method decides if either the selection box,
// MouseSpring, or a "partial spring" needs to be
// drawn and then does the drawing.
if ( !leftButtonPressed ) return;
if ( ms->isActive( ) ) {
painter.setPen( red );
Particle* const p = ms->getTarget( );
painter.drawLine( ( int )p->x[Particle::POSN],
( int )p->x[Particle::POSN+1], thisx, thisy );
return;
}
if ( mousemode == MAKESPRING ) {
//connect the pointer with particle
if ( clicked != 0 ) {
if( sprType ) painter.setPen( sprType->getColor( ) );
else painter.setPen( Qt::SolidLine );
painter.drawLine( ( int )clicked->x[0], ( int )clicked->x[1],
thisx, thisy );
}
}
else {
// draw the selection box.
drawBox( painter );
}
}
360 void ParticleField::drawConstraints( QPainter& p ) {
if( !cs ) return;
unsigned int max = cs->size( );
const Constraint* c = 0;
const Distance * d = 0;
const Circle* circ = 0;
int x1, y1, x2, y2;
QPen pen( black, 2 );
p.setPen( pen );
p.setBrush( NoBrush );
for ( unsigned int i = 0; i < max ;i++ ) {
c = cs->getConstraint( i );
if ( c ) d = dynamic_cast<const Distance*>( c );
if( d ) {
const Particle& p1 = d->getParticle1( );
const Particle& p2 = d->getParticle2( );
x1 = ( int ) p1.x[Particle::POSN];
y1 = ( int ) p1.x[Particle::POSN+1];
x2 = ( int ) p2.x[Particle::POSN];
y2 = ( int ) p2.x[Particle::POSN+1];
p.setPen( pen );
p.drawLine( x1, y1, x2, y2 );
}
else {
if( c ) circ = dynamic_cast<const Circle*>( c );
if( circ ) {
const Circle::Data& cd = circ->getData( );
float r = sqrt( cd.r );
//std::cout << cd.x << " "<< cd.y <<" " << cd.z<<
//" " << cd.r << std::endl;
p.drawEllipse( ( int ) ( cd.x - r ), ( int ) ( cd.y - r ),
( int ) ( 2*r ), ( int ) ( 2*r ) );
}
}
d = 0; c= 0;circ = 0;
}
}
401 void ParticleField::drawGrid( QPainter& p ){
p.setPen( white );
//horizontal lines
int h = height( ); int w = width( );
for( int i = 0; i < h; i+=gridSize )
p.drawLine( 0, i, w, i );
//vertical lines
for( int j = 0;j < w;j+= gridSize )
p.drawLine( j, 0, j, h );
}
412 void ParticleField::update( ) {
try {
e->update( deltaT ); // do the physics
}
catch( Matrix::Error& e ) {
std::cerr << "Simulation Aborted"<< std::endl;
runSim( false );
emit requestReload( );
}
//deal with tracer
if( tracing ) updateTrace( );
//draw the screen
QPaintEvent* pev = new QPaintEvent( rect( ) );
paintEvent( pev );
delete pev;
}
433 void ParticleField::setDeltaT( float val ) {
deltaT = val;
}
438 void ParticleField::init( ) {
QSize s = size( );
pix.resize( s );
tracemap.resize( s );
// put a background on the tracemap
QPainter p( &tracemap );
p.fillRect( rect( ), *bgBrush );
}
449 void ParticleField::clearAll( ) {
if( ps ) ps->clear( );
if( cs ) cs->clear( );
if( tracing ) toggleTrace( );
ms->releaseParticle( );
}
457 void ParticleField::clearSprings( ) {
ps->clearSprings( );
}
462 void ParticleField::setViscousDrag( float val ) {
drag->setCoeff( val );
emit action( "viscous drag changed" );
}
468 void ParticleField::setGravity( int val ) {
gravity->setY( ( float )val );
emit action( "gravity changed" );
}
474 void ParticleField::setWallRestitution( float val ){
float b;
if( val < 0 ) b = val;
else b = -val;
for ( int i = 0;i < 4; i++ ) {
borders[i]->coeff( b );
}
}
484 void ParticleField::setUpdateTime( int val ){
interval = val;
if( running ) timer->changeInterval( interval );
}
489 void ParticleField::getToolBarSettings( SimData* sd ){
sd->dt = deltaT;
sd->du = interval;
sd->gravity = ( int )gravity->getY( );
sd->wallrest = borders[0]->coeff( );
sd->drag = drag->getCoeff( );
}
500 void ParticleField::setSpringType( SpringType* ns ) {
sprType = ns;
}
505 void ParticleField::buildBorders( ) {
//ground
float* posn = new float[2];
posn[0] = 300.0; posn[1] = 600.0;
float* normal = new float[2];
normal[0] = 0.0; normal[1] = -1.0;
borders[0] = new D2_Plane( ps, posn, normal, -0.7 );
//ceiling
posn = new float[2];
posn[0] = 300.0; posn[1] = 0.0;
normal = new float[2];
normal[0] = 0.0; normal[1] = 1.0;
borders[1] = new D2_Plane( ps, posn, normal, -0.7 );
//left wall
posn = new float[2];
posn[0] = 0.0; posn[1] = 300.0;
normal = new float[2];
normal[0] = 1.0; normal[1] = 0.0;
borders[2] = new D2_Plane( ps, posn, normal, -0.7 );
//right wall
posn = new float[2];
posn[0] = 600.0; posn[1] = 300.0;
normal = new float[2];
normal[0] = -1.0; normal[1] = 0.0;
borders[3] = new D2_Plane( ps, posn, normal, -0.7 );
for ( int i = 0; i <4 ;i++ ) {
ps->addCollider( borders[i] );
}
}
542 bool ParticleField::makeSpring( QMouseEvent * me ){
Particle* p = selectParticle( me );
if ( p == 0 || clicked == 0 || p == clicked )
return false;
Spring* f;
if( !sprType )
f = new Spring( p, clicked );
else {
f = new Spring( p, clicked, sprType->getKs( ),
sprType->getKd( ), sprType->getLen( ) );
f->setColor( sprType->getColor( ) );
}
emit action( "made spring" );
ps->addForce( f );
return true;
}
560 void ParticleField::addParticle( int x, int y, bool fixed ) {
if ( snapToGrid ){
int q = x % gridSize;
int m = x / gridSize;
if( q < gridSize - q ) x = m*gridSize;
else x = ( m+1 )*gridSize;
q = y % gridSize;
m = y / gridSize;
if( q < gridSize - q ) y = m*gridSize;
else y = ( m+1 )*gridSize;
}
Particle* q = ps->addParticle( ( float )x, ( float ) y, 0.0 );
if( fixed ) {
// fix the particle in place, 2 diffn ways
if( mode == COLLISIONS )
q->setFixed( true );
else
cs->fixParticle( *q );
}
}
586 void ParticleField::boxSelect( QMouseEvent* me ) {
int minx, maxx, miny, maxy, px, py;
//cout << "entering boxSelect" << endl;
if( startx < me->x( ) ) { minx = startx; maxx = me->x( );}
else { minx = me->x( ); maxx = startx;}
if( starty < me->y( ) ) { miny = starty; maxy = me->y( );}
else { miny = me->y( ); maxy = starty;}
Particle* p;
unsigned int num = selected->size( );
//cout << selected->size( );
for ( unsigned int i = 0; i < ps->size( ); i++ ) {
p = ( *ps )[i];
px = ( int ) p->x[Particle::POSN];
py = ( int ) p->x[Particle::POSN+1];
//cout << px<< " " << py << endl;
if ( px > minx && px < maxx && py > miny && py < maxy ) {
//cout<< "inserting" << endl;
selected->insert( p );
}
}
if ( num != selected->size( )|| selected->size( ) == 0 )
emit numParticlesSelected( selected->size( ) );
}
// Assumes tracemap has been set to the correct dimensions
615 void ParticleField::updateTrace( ) {
if ( tracer && tracing ) {
QPainter p( &tracemap );
p.setPen( tracePen );
p.drawLine( ( int ) tracer->x[Particle::POSN],
( int )tracer->x[Particle::POSN+1],
( int )tracer->x[Particle::LASTX],
( int )tracer->x[Particle::LASTX+1] );
}
}
627 void ParticleField::setSelectMode( ) {
// box select allowed in Collisions mode from 28-1-00
/*if ( mode == COLLISIONS ) {
emit action( "Sorry, box select only in Constraint mode." );
return;
} */
mousemode = SELECT;
emit action( "box select mode." );
}
639 void ParticleField::setSpringMode( ) {
mousemode = MAKESPRING;
selected->clear( ); //these two lines prob not nesc.
emit numParticlesSelected( 0 );
emit action( "make spring mode." );
}
ParticleField::MouseMode
648 ParticleField::getMouseMode( ) {
return mousemode;
}
653 void ParticleField::setCMode( int newMode ) {
if ( ( int ) mode == newMode ) {
if ( mode == COLLISIONS )
emit action( "Already in Collisions mode." );
else
emit action( "Already in Constraints mode." );
return;
}
if ( newMode < 0 || newMode > 1 ){
std::cout << "invalid parameter to ParticleField::setCMode( )." <<
"\npassed value: " << newMode << std::endl;
exit( 1 );
}
mode = static_cast<Mode>( newMode );
if ( mode == COLLISIONS ) {
//mousemode = MAKESPRING;
emit action( "Collisions mode - no constraints possible." );
cs = ps->enableConstraints( false );
}
else {
cs = ps->enableConstraints( true );
emit action( "Constraints mode - no borders." );
}
clearAll( );
}
ParticleField::Mode
683 ParticleField::getCMode( ) {
return mode;
}
ParticleField::SolverType
689 ParticleField::getSolverType( ) {
return st;
}
694 int ParticleField::getGridSpacing( ) {
return gridSize;
}
698 void ParticleField::setSolver( int type ) {
if( ( ( int ) st ) == type ) {
emit action( "Already using that kind of ODE Solver" );
return;
}
switch( type ) {
case 0 : delete e;
e = new Euler( ps );
st = EULER;
break;
case 1 : delete e;
e = new MidPoint( ps );
st = MIDPOINT;
break;
case 2 : delete e;
e = new RungeKutta4( ps );
st = RK;
break;
default: std::cerr << "invalid parameter to ParticleField::setSolver( )." <<
"\npassed value: " << type << std::endl;
exit( 1 );
}
}
725 void ParticleField::runSim( bool on ) {
if( !running && on ) {
timer->start( interval );
running = true;
emit simStarted( running );
}
else if( running && !on ){
timer->stop( );
running = false;
emit simStarted( running );
}
}
739 void ParticleField::makeConstraint
( ConstraintType c, float f ){
if( mode != CONSTRAINTS ) return;
Particle* p = 0;
Circle::Data d( 0.0, 0.0, 0.0, 0.0 );
selIter i = selected->begin( );
selIter l = i;
switch( c ) {
case FIXED : while( i != selected->end( ) ){
cs->fixParticle( *( *i ) );
i++;
}
break;
case HORIZ : while( i != selected->end( ) ) {
cs->addDimensionConstraint( *( *i ), 1 );
i++;
}
break;
case VERT : while( i != selected->end( ) ) {
cs->addDimensionConstraint( *( *i ), 0 );
i++;
}
break;
case CIRCLE : p = *i;
d.x = p->x[Particle::POSN];
d.y = p->x[Particle::POSN+1];
d.z = 0.0;
d.r = f*f;
p->x[Particle::POSN+1] += f;
cs->addCircleConstraint( *p, d );
break;
case DISTANCE : if( selected->size( ) != 2 ) return;
l++;
cs->addDistConstraint( *( *i ), *( *l ), f );
break;
case DEF_DISTANCE : if( selected->size( ) != 2 ) return;
l++;
{ Particle* p1 = *i;
Particle* p2 = *l;
float dx = p1->x[Particle::POSN] -
p2->x[Particle::POSN];
float dy = p1->x[Particle::POSN+1] -
p2->x[Particle::POSN+1];
float len = sqrtf( dx*dx+dy*dy );
cs->addDistConstraint( *p1, *p2, len ); }
break;
default : std::cerr << "ParticleField::makeConstraint( )" <<
" fallen through case !!!" << std::endl;
}
emit action( "made constraint" );
QPaintEvent pev( rect( ) );
paintEvent( &pev );
}
801 void ParticleField::deleteSelectedParticles( ){
// deal with the Mouse_Spring first.
S_Iter k = selected->find( ms->getTarget( ) );
if( k != selected->end( ) ) ms->releaseParticle( );
// deal with deleting the tracer
if( tracing ) {
k = selected->find( tracer );
if( k != selected->end( ) )toggleTrace( );
}
S_Iter i = selected->begin( );
S_Iter j = i;
while ( i != selected->end( ) ){
ps->removeParticle( *i );
j = i++;
selected->erase( j );
}
if( !running ) {
QPaintEvent pev( rect( ) );
paintEvent( &pev );
}
}
827 void ParticleField::toggleTrace( ) {
if( tracing ) { // stop tracing
tracer = 0;
tracing = false;
emit action( "tracing stopped" );
}
else {
// see if just one particle is selected
if ( selected->size( ) == 1 ){
drawTrace = true; tracing = true;
S_Iter i = selected->begin( );
tracer = *i;
emit action( "tracing started" );
}
else
emit action( "select just one particle to trace" );
}
}
848 void ParticleField::clearTrace( ) {
QPainter p( &tracemap );
p.fillRect( rect( ), *bgBrush );
emit action( "trace cleared" );
if( !tracing ) drawTrace = false;
}
856 void ParticleField::save( QDataStream& qds ) {
qds << interval;
qds << deltaT;
qds << borders[0]->coeff( );
qds << *ps;
}
865 void ParticleField::load( QDataStream& qds ) {
clearAll( );
//these few bits the ParticleField takes care of.
qds >> interval;
qds >> deltaT;
float f;
qds >> f; setWallRestitution( f );
ps->load( qds, drag, gravity );
//mousespring deleted by load operation
ms = new Mouse_Spring( ps );
ps->addForce( ms );
cs = ps->getCS( );
if ( cs ) mode = CONSTRAINTS;
}
//toggle functionality
885 void ParticleField::setSnapToGrid( ) {
snapToGrid = !snapToGrid;
}
890 void ParticleField::setDisplayGrid( ) {
displayGrid = !displayGrid;
}
895 void ParticleField::setGridSpacing( int space ) {
if( space < 10 ) space = 10;
if( space > 150 ) space = 150;
gridSize = space;
}
#ifndef cl_particleField
#define cl_particleField
#include <iostream>
#include <set>
#include <qtimer.h>
#include <qpixmap.h>
#include <qpoint.h>
#include <qwidget.h>
#include "Particle.h"
#include "Force.h"
#include "ODESolver.h"
#include "Plane.h"
#include "Mouse_Spring.h"
#include "ConstraintChooser.h"
#include "SpringType.h"
#include "SimData.h"
typedef std::set<Particle*>::iterator selIter;
24 class ParticleField : public QWidget
{
Q_OBJECT
public:
ParticleField( QWidget *parent=0, const char *name=0 );
~ParticleField( );
//QSizePolicy sizePolicy( ) const;
void init( );
enum Mode{ COLLISIONS, CONSTRAINTS};
enum MouseMode { MAKESPRING, SELECT};
enum SolverType { EULER = 0, MIDPOINT = 1, RK = 2 };
37 MouseMode getMouseMode( );
38 Mode getCMode( );
39 SolverType getSolverType( );
40 int getGridSpacing( );
42 signals:
// connected to the Window, displays messages on the
// message bar
void action( char* );
// connected to the constraintchooser dialog, so it
// can display the correct sorts of constraints
// depending on the number of particles selected.
50 void numParticlesSelected( unsigned int n );
// connected to the simToolBar, so that the GO button
// will change state when sim is started from the keyboard.
54 void simStarted( bool on );
/**
* This is emitted when the simulation has crashed,
* and the simulation file ( if there is one ) should
* be reloaded.
*/
61 void requestReload( );
public slots:
void update( );
void setDeltaT( float val );
void clearAll( );
void clearSprings( );
void setSelectMode( );
void setSpringMode( );
void setCMode( int newMode );
void setSolver( int type );
void runSim( bool on );
void setWallRestitution( float val );
void setGravity( int val );
void setViscousDrag( float val );
void setUpdateTime( int );
void setSpringType( SpringType* );
void getToolBarSettings( SimData* );
void makeConstraint( ConstraintType, float );
void deleteSelectedParticles( );
void toggleTrace( );
void clearTrace( );
void save( QDataStream& );
void load( QDataStream& );
void setSnapToGrid( );
void setDisplayGrid( );
void setGridSpacing( int );
protected:
void paintEvent( QPaintEvent * );
virtual void mousePressEvent ( QMouseEvent * );
virtual void mouseReleaseEvent ( QMouseEvent * );
virtual void mouseMoveEvent( QMouseEvent *me );
virtual void keyPressEvent ( QKeyEvent * ke );
private:
Particle* selectParticle( QMouseEvent * );
void drawParticles( QPainter & );
void drawSprings( QPainter & );
void drawSpringMatt( QPainter& paint, float pZeroX ,
float pZeroY, float pOneX, float pOneY );
void drawBox( QPainter& );
void drawMouseArt( QPainter& );
void drawConstraints( QPainter& );
void drawGrid( QPainter& );
void buildBorders( );
bool makeSpring( QMouseEvent * me );
void addParticle( int x, int y, bool fixed );
// put all the particles within the box
// in the selected set.
void boxSelect( QMouseEvent * );
// plot another point in the trace
void updateTrace( );
ParticleSystem* ps;
ODESolver* e;
ConstraintSystem * cs;
Particle* clicked;
float deltaT;
bool leftButtonPressed;
SpringType* sprType;
bool running;
QTimer* timer;
int interval;
D2_Plane* borders[4];
Global_Drag* drag;
Const_Field* gravity;
Mouse_Spring* ms;
std::set<Particle*> *selected;
typedef std::set<Particle*>::iterator S_Iter;
Mode mode;
MouseMode mousemode;
SolverType st;
int startx, starty, thisx, thisy;
QColor* bg;
QBrush* bgBrush;
QPixmap pix;
// to do the tracing
Particle* tracer;
QPixmap tracemap;
bool tracing, drawTrace;
QColor tracePen;
//for snap particle to grid functionality
bool snapToGrid, displayGrid;
int gridSize;
bool drawComplexSpring;
};
#endif //cl_particleField
1 #include "Plane.h"
4 D2_Collider::D2_Collider( ParticleSystem* parent ) :
ps( parent ){
}
9 D2_Collider::D2_Collider( const D2_Collider& c ):
ps( c.ps ){}
D2_Collider&
14 D2_Collider::operator=( const D2_Collider& c ) {
ps = c.ps;
return *this;
}
// the position argument should be a dynamically allcated
// array of 2 floats [x1, y1].which the plane passes through
// the normal is an array of two floats.
26 D2_Plane::D2_Plane( ParticleSystem* parent,
float* position, float* normal, float coeffOfRest ) :
D2_Collider( parent ) {
if( parent == 0 || position == 0 || normal == 0 ) {
std::cout << "Plane::Plane( ) called with null "<< "argument" <<
std::endl;
exit( 1 );
}
if( coeffOfRest >= 0 ) {
std::cout << "Plane::Plane : " <<
"coeffOfReff must be < 0" << std::endl;
}
posn = position;
norm = normal;
r = coeffOfRest;
}
43 D2_Plane::~D2_Plane( ) {
delete[] posn;
delete[] norm;
}
48 D2_Plane::D2_Plane( const D2_Plane& p ) :
D2_Collider( p.ps ) {
norm = new float[2];
posn = new float[2];
operator=( p );
}
55 D2_Plane& D2_Plane::operator=( const D2_Plane& p ) {
if ( this != &p ) {
norm[0] = p.norm[0]; norm[1] = p.norm[1];
posn[0] = p.posn[0]; posn[1] = p.posn[1];
r = p.r;
}
return *this;
}
65 void D2_Plane::coeff( float val ) {
r = val;
}
70 float D2_Plane::coeff( ) const {
return r;
}
76 void D2_Plane::solveCollisions( ) {
//first test for a collision
Particle* p;
float temp[2];
float vtang[2];
float outnorm[2];
outnorm[0] = -1.0 * norm[0];
outnorm[1] = -1.0 * norm[1];
float result;
static int j = Particle::VEL;
for ( unsigned int i = 0; i < ps->size( ); i++ ) {
p = ( *ps )[i];
temp[0] = p->x[0] - posn[0];
temp[1] = p->x[1] - posn[1];
result = ( temp[0] ) * ( norm[0] ) + ( temp[1] )*( norm[1] );
if ( result <= 0 ) {
// deal with the response
// find the normal component of the velocity vector
//PR( p->x[0] ); PR( p->x[1] );
//PR( p->x[j] ); PR( p->x[j+1] );
result = ( outnorm[0] ) * ( p->x[j] ) +
( outnorm[1] ) * ( p->x[j+1] );
if( result < 0.0 ) continue;
temp[0] = result * ( outnorm[0] );
temp[1] = result * ( outnorm[1] );
//PR( temp[0] ); PR( temp[1] );
//temp now holds the normal component
// of velocity.
vtang[0] = p->x[j] - temp[0];
vtang[1] = p->x[j+1] - temp[1];
//check for contact
result = sqrtf( temp[0] * temp[0] + temp[1]* temp[1] );
//PR( result );
if ( result < 10.0 ) {
temp[0] = 0.0;
temp[1] = 0.0;
}
//PR( vtang[0] ); PR( vtang[1] );
// vtang is the component of velocity tangent
// to the plane.
p->x[j] = vtang[0] + ( r * temp[0] );
p->x[j+1] = vtang[1] + ( r * temp[1] );
//PR( p->x[Particle::VEL] ); PR( p->x[Particle::VEL+1] );
//cout << endl;
}
}
}
1 #ifndef cl_plane
#define cl_plane
#include "Particle.h"
#include <iostream>
#include <stdlib.h>
#include <cmath>
#define PR( x ) cout << #x " = " << x << "\n";
10 class D2_Collider {
public:
13 D2_Collider( ParticleSystem* parent );
14 virtual ~D2_Collider( ){}
15 D2_Collider( const D2_Collider& d );
16 virtual D2_Collider& operator=( const D2_Collider& );
17 virtual void solveCollisions( ) = 0;
protected:
20 ParticleSystem* ps;
};
23 class D2_Plane : public D2_Collider{
public:
//note that normal muat be a unit vector!
28 D2_Plane( ParticleSystem* parent, float* position,
float* normal, float coeffOfRest );
30 virtual ~D2_Plane( );
31 D2_Plane( const D2_Plane& p );
32 virtual D2_Plane& operator=( const D2_Plane& );
33 virtual void solveCollisions( );
35 void coeff( float val );
36 float coeff( ) const ;
private:
float r;
float* posn;
float* norm;
};
#endif
1 #include "ODESolver.h"
#include <iostream>
const float RungeKutta4::factor[] = {0.5, 1.0};
6 RungeKutta4::RungeKutta4( ParticleSystem * p ) :
ODESolver( p ){
if( ( num_pcls = ps->size( ) ) > 0 ) {
yn = new float[3*num_pcls];
result = new float[6*num_pcls];
}
else {
yn = 0;
result = 0;
}
yn_size = num_pcls;
sixth = 1.0 / 6.0;
}
22 RungeKutta4::~RungeKutta4( ) {
delete[] yn;
delete[] result;
}
27 void RungeKutta4::update( float deltaT ) {
//cout << "RungeKutta4: entering update" << endl;
// make sure yn and k1 is big enough
if( ( num_pcls = ps->size( ) ) == 0 ) return;
if ( num_pcls > yn_size ) {
yn_size = num_pcls;
delete [] yn; delete [] result;
yn = new float[3*yn_size];
result = new float[6*num_pcls];
}
// save the original velocity
// values in yn
Particle *p;
unsigned int j, k; j = 0;
for( unsigned int i = 0; i < num_pcls; i++ ) {
p = ( *ps )[i];
for( k = 0; k < 3; k++ )
yn[j+k] = p->x[Particle::VEL+k];
j+=3;
//keep the previous position here
for( k = 0; k < 3; k++ )
p->x[Particle::LASTX+k] = p->x[k];
}
// do the first Euler bit to
halfDel = deltaT / 2.0;
ps->calculateForces( );
float a;
for ( unsigned int i = 0; i < num_pcls; i++ ) {
p = ( *ps )[i];
if ( p->isFixed( ) ) continue;
//cout << "derivative vector" << endl;
// get the "derivative vector" and scale it.
for ( int j = 0 ; j < 3 ;j++ ) {
result[i*6+j] = deltaT*( p->x[Particle::VEL+j] );
p->x[Particle::POSN+j] +=
( halfDel*( p->x[Particle::VEL+j] ) );
}
for ( int j = 0 ; j < 3 ;j++ ){
a = ( p->x[Particle::FORCE+j] )/ p->mass;
result[i*6+j+3] = deltaT*a;
p->x[Particle::VEL+j] += ( halfDel * a );
}
}
ps->calculateForces( );
for( unsigned int k = 0; k < 2; k++ ) {
for ( unsigned int i = 0; i < num_pcls; i++ ) {
p = ( *ps )[i];
if ( p->isFixed( ) ) continue;
for ( int j = 0 ; j < 3 ;j++ ) {
result[i*6+j] += 2*deltaT*( p->x[Particle::VEL+j] );
p->x[Particle::POSN+j] = ( p->x[Particle::LASTX+j] )+
( factor[k]*deltaT*( p->x[Particle::VEL+j] ) );
}
for ( int j = 0 ; j < 3 ;j++ ){
a = deltaT * ( ( p->x[Particle::FORCE+j] )/ p->mass );
result[i*6+j+3] += 2*a;
p->x[Particle::VEL+j] = ( yn[( i*3 )+j] )+( factor[k] * a );
}
}
ps->calculateForces( );
}
for ( unsigned int i = 0; i < num_pcls; i++ ) {
p = ( *ps )[i];
if ( p->isFixed( ) ) continue;
//cout << "derivative vector" << endl;
// get the "derivative vector" and scale it.
for ( int j = 0 ; j < 3 ;j++ ) {
result[i*6+j] += deltaT*( p->x[Particle::VEL+j] );
}
for ( int j = 0 ; j < 3 ;j++ ){
a = ( p->x[Particle::FORCE+j] )/ p->mass;
result[i*6+j+3] += deltaT*a;
}
}
for ( unsigned int i = 0; i < num_pcls; i++ ) {
p = ( *ps )[i];
if ( p->isFixed( ) ) continue;
for ( int j = 0 ; j < 3 ;j++ )
p->x[Particle::POSN+j] =
( p->x[Particle::LASTX+j] + ( sixth*( result[i*6+j] ) ) );
for ( int j = 0 ; j < 3 ;j++ )
p->x[Particle::VEL+j] =
( yn[( i*3 )+j] ) + ( sixth*( result[i*6+j+3] ) );
}
//cout << "RungeKutta4: leaving update" << endl;
}
1 #ifndef SIM_DATA
#define SIM_DATA
struct SimData{
float dt;
int du;
int gravity;
float wallrest;
float drag;
};
#endif
1 #include "SimToolBar.h"
3 SimToolBar::SimToolBar( QMainWindow * parent, const char * name ):
QToolBar( parent, name ) {
run = new QToolButton( this, "run" );
run->setToggleButton( true );
connect( run, SIGNAL( toggled( bool ) ),
this, SIGNAL( simModeChanged( bool ) ) );
QPixmap pm;
bool success = loadPixmap( pm );
if( success )
run->setPixmap( pm );
a = new QLabel( " delta t ", this, "a" );
dt = new ZeroToOne( this, "dt" );
connect( dt, SIGNAL( valueChangedf( float ) ),
this, SIGNAL( deltaTChanged( float ) ) );
dt->setValue( 6 );
b = new QLabel( " update t ", this, "b" );
update = new QSpinBox( this, "update" );
connect( update, SIGNAL( valueChanged( int ) ),
this, SIGNAL( updateTimeChanged( int ) ) );
update->setValue( 30 );
c = new QLabel( " wall bounce ", this, "c" );
walls = new ZeroToOne( this, "walls" );
connect( walls, SIGNAL( valueChangedf( float ) ),
this, SIGNAL( wallcoeffChanged( float ) ) );
walls->setValue( 70 );
d = new QLabel( " gravity ", this, "d" );
grav = new QSpinBox( this, "grav" );
connect( grav, SIGNAL( valueChanged( int ) ),
this, SIGNAL( gravityChanged( int ) ) );
grav->setValue( 35 );
e = new QLabel( " drag ", this, "e" );
drag = new ZeroToOne( this, "drag" );
connect( drag, SIGNAL( valueChangedf( float ) ),
this, SIGNAL( dragChanged( float ) ) );
drag->setValue( 8 );
}
50 void SimToolBar::setButtonOn( bool on ) {
run->setOn( on );
}
56 void SimToolBar::setValues( SimData& sd ){
dt->setValuef( sd.dt );
update->setValue( sd.du );
walls->setValuef( fabs( sd.wallrest ) );
grav->setValue( sd.gravity );
drag->setValuef( sd.drag );
}
65 bool SimToolBar::loadPixmap( QPixmap& pix ) {
QString pixFile;
bool loadSuccess = false;
if( !getResourcePath( pixFile, GO_PIXMAP ) ) {
goto load_error;
}
loadSuccess = pix.load( pixFile );
if( !loadSuccess )
goto load_error;
return true;
load_error:
std::cerr << "Could not load " << pixFile << std::endl;
return false;
}
1
#ifndef cl_SimToolBar
#define cl_SimToolBar
#include <qtoolbar.h>
#include <qlabel.h>
#include <qtoolbutton.h>
#include <qpixmap.h>
#include "CustomSpinBox.h"
#include <math.h>
#include <qfile.h>
#include "SimData.h"
#include "Environ.h"
#define GO_PIXMAP "go.png"
18 class SimToolBar : public QToolBar {
Q_OBJECT
public:
SimToolBar( QMainWindow * parent = 0, const char * name = 0 );
void setValues( SimData& );
public slots:
void setButtonOn( bool on );
signals:
void deltaTChanged( float val );
void updateTimeChanged( int val );
void wallcoeffChanged( float val );
void gravityChanged( int val );
void simModeChanged( bool started );
void dragChanged( float val );
private:
ZeroToOne* dt;
QSpinBox* update;
ZeroToOne* walls;
QSpinBox* grav;
ZeroToOne* drag;
QLabel *a, *b, *c, *d, *e;
QToolButton * run;
// loads pixmap from resources directory
// returns true on success
bool loadPixmap( QPixmap& pix );
};
#endif
1 #include "SpringType.h"
#include "qcolordialog.h"
//------------------------------------
// ColorChit Methods
//------------------------------------
9 ColorChit::ColorChit( QWidget * parent, const char * name ):
QWidget( parent, name ), c( 0, 0, 0 ){
setFixedSize( 40, 20 );
setBackgroundColor( c );
}
17 const QColor& ColorChit::color( ) const {
return c;
}
22 void ColorChit::color( const QColor& color ) {
c = color;
setBackgroundColor( c );
}
28 void ColorChit::mouseDoubleClickEvent( QMouseEvent* me ){
QColor col = c;
QColor newcol = QColorDialog::getColor( col, this, "colorDialog" );
if( newcol.isValid( ) && ( newcol != c ) ) {
c = newcol;
setBackgroundColor( c );
emit colorChanged( c );
}
}
//---------------------------------------
// SpringToolBar Methods
//---------------------------------------
44 SpringToolBar::SpringToolBar( QMainWindow * parent, const char * name ):
QToolBar( parent, name ){
newtype = new QPushButton( "New Type", this, "newTypeButton" );
cb = new QComboBox( true, this, "spring type list" );
cb->setMinimumWidth( 150 );
cb->setInsertionPolicy( QComboBox::AtCurrent );
a = new QLabel( " ks:", this, "a" );
ks = new ZeroToTen( this, "ksSpinBox" );
b = new QLabel( " kd:", this, "b" );
kd = new ZeroToOne( this, "kdSpinBox" );
c = new QLabel( " length:", this, "c" );
rl = new ZeroTo500( this, "" );
addSeparator( );
chit = new ColorChit( this, "chitbaby" );
addSeparator( );
newTypeCount = 0;
types = new std::list<SpringType*>( );
current = 0;
loadDefaults( );
connect( cb, SIGNAL( activated( int ) ), this, SLOT( typeChanged( int ) ) );
connect( ks, SIGNAL( valueChangedf( float ) ), this,
SLOT( ksChanged( float ) ) );
connect( kd, SIGNAL( valueChangedf( float ) ), this,
SLOT( kdChanged( float ) ) );
connect( rl, SIGNAL( valueChangedf( float ) ), this,
SLOT( lenChanged( float ) ) );
connect( chit, SIGNAL( colorChanged( QColor& ) ), this,
SLOT( colorChanged( QColor& ) ) );
connect( newtype, SIGNAL( clicked( ) ), this,
SLOT( newSpringType( ) ) );
}
84 SpringToolBar::~SpringToolBar( ){
Iter i = types->begin( );
while( i != types->end( ) ) {
delete *i; i++;
}
delete types;
}
93 void SpringToolBar::init( ) {
if ( ! types->empty( ) )
typeChanged( 0 );
}
99 void SpringToolBar::loadDefaults( ) {
QString filename;
if( !getResourcePath( filename, SPRING_TYPE_FILE ) ) { // !!! FATAL ERROR !!!
std::cerr << "Required file " << filename.latin1( ) << " not found, "
<< "exiting." << std::endl;
exit( 1 );
}
QFile f( filename );
if( !f.open( IO_ReadOnly ) ) {
std::cerr << "Could not open " << ( const char * )filename
<< "for reading, exiting." << std::endl;
exit( 1 );
}
SpringType* st;
QDataStream data( &f );
while( !data.atEnd( ) ){
st = new SpringType( );
data >> ( *st );
types->push_back( st );
//cout << ( *st );
cb->insertItem( st->getName( ) );
}
}
128 void SpringToolBar::typeChanged( int index ) {
//cout << "Entering SpringToolBar::typeChanged( )" << endl;
QString s = cb->currentText( );
Iter i = types->begin( );
while( i != types->end( ) ) {
if( ( *i )->getName( ) == s ) break;
i++;
}
if( i == types->end( ) ) {
//cout << "error in SpringToolBar::typeChanged( ) " <<endl;
//exit( 1 );
nameChanged( index, s );
return;
}
if ( current == *i ) return; //just picked the same one
current = *i;
ks->setValuef( current->getKs( ) );
kd->setValuef( current->getKd( ) );
rl->setValuef( current->getLen( ) );
chit->color( current->getColor( ) );
emit typeAltered( current );
//cout << "Leaving SpringToolBar::typeChanged( )" << endl;
}
155 void SpringToolBar::ksChanged( float val ){
if( current ) current->setKs( val );
}
160 void SpringToolBar::kdChanged( float val ){
if( current ) current->setKd( val );
}
165 void SpringToolBar::lenChanged( float val ){
if( current ) current->setLen( val );
}
170 void SpringToolBar::colorChanged( QColor& col ){
if( current ) current->setColor( col );
}
//called if the user has edited one of the SpringType
//names in the combo box
177 void SpringToolBar::nameChanged( int pos, const QString& s ){
//cout << "Entering SpringToolBar::nameChanged( )" << endl;
Iter i = types->begin( );
for( int j = 0; j < pos; j++ ) i++;
current = *i;
current->setName( s );
}
186 void SpringToolBar::newSpringType( ) {
QString a = "New Type";
QString b; b.setNum( newTypeCount++ );
a += b;
SpringType* st = new SpringType( a, current->getColor( ),
current->getKs( ), current->getKd( ), current->getLen( ) );
types->push_back( st );
current = st;
cb->insertItem( a );
cb->setCurrentItem( ( cb->count( ) ) - 1 );
emit typeAltered( current );
// dont emit signal typeAltered because all values have
//changed, it will be emited once something other than
// the name has changed - emitten by typeChanged( )
}
1 #include "SpringType.h"
3 SpringType::SpringType( const QString& name, const QColor& color,
float iks, float ikd, float restlength ) :
col( color ), n( name ), ks( iks ), kd( ikd ), len( restlength ){}
//no color specified means default color is black.
9 SpringType::SpringType( const QString& name, float iks, float ikd,
float restlength ) :
col( 0, 0, 0 ), n( name ) , ks( iks ), kd( ikd ), len( restlength ){}
14 void SpringType::setColor( QColor& newColor ) {
col = newColor;
}
19 void SpringType::setColor( int red, int green, int blue ) {
col.setRgb( red, green, blue );
}
24 const QColor& SpringType::getColor( ) const{
return col;
}
29 void SpringType::setName( const QString & str ) {
n = str;
}
33 const QString& SpringType::getName( ) const {
return n;
}
37 void SpringType::setCoeffs( float* array ) {
if( array[0] >= 0 ) ks = array[0];
if( array[1] >= 0 ) ks = array[1];
if( array[2] >= 0 ) ks = array[2];
}
43 const void SpringType::getCoeffs( float* springk, float* damping,
float* restlen ) const {
*springk = ks;
*damping = kd;
*restlen = len;
}
// global functions
52 QDataStream& operator<<( QDataStream& ds, const SpringType& st ) {
ds << st.col << st.n << st.ks << st.kd << st.len;
return ds;
}
58 QDataStream& operator>>( QDataStream& ds, SpringType& st ) {
ds >> st.col >> st.n >> st.ks >> st.kd >> st.len;
return ds;
}
64 std::ostream& operator<<( std::ostream& os, const SpringType& st ) {
os << st.n.latin1( ) << std::endl << st.ks << " " << st.kd << " "<<st.len << " ";
int r, g, b;
st.col.rgb( &r, &g, &b );
os << r <<" " << g << " " << b << std::endl;
return os;
}
1
#ifndef CL_SPRINGTYPE
#define CL_SPRINGTYPE
#include <qtoolbar.h>
#include <qcombobox.h>
#include <qspinbox.h>
#include <qlayout.h>
#include <qpushbutton.h>
#include <qlabel.h>
#include <qstring.h>
#include <qcolor.h>
#include <qdatastream.h>
#include <iostream>
#include <qfile.h>
#include <list>
#include "CustomSpinBox.h"
#include "Environ.h"
#define SPRING_TYPE_FILE "springtypes"
24 class SpringType {
public:
27 SpringType( const QString& name, const QColor& color,
float iks=0.5, float ikd=0.5, float restlength=100.0 );
29 SpringType( const QString& name = "", float
iks=0.5, float ikd=0.5, float restlength=100.0 );
32 void setColor( QColor & );
// 0 < all args < 255
34 void setColor( int red, int green, int blue );
35 const QColor & getColor( ) const ;
37 void setName( const QString & );
38 const QString& getName( ) const ;
// pass a length 3 array. If any of the
// entries are negative, they are left unchanged.
// order is spring constant, damping, length
43 void setCoeffs( float* array );
44 const void getCoeffs( float* springk, float* damping,
float* restlen ) const;
47 const float getKs( ){return ks;}
48 const float getKd( ){return kd;}
49 const float getLen( ){return len;}
51 void setKs( float newval ){ks = newval;}
52 void setKd( float newval ){kd = newval;}
53 void setLen( float newval ){len = newval;}
// global functions
56 friend QDataStream& operator<<( QDataStream& ds,
const SpringType& st );
58 friend QDataStream& operator>>( QDataStream& ds,
SpringType& st );
60 friend std::ostream& operator<< ( std::ostream& os, const SpringType& st );
private:
63 QColor col;
64 QString n;
float ks;
float kd;
float len;
};
75 class ColorChit : public QWidget {
Q_OBJECT
public:
ColorChit( QWidget * parent=0, const char * name=0 );
const QColor & color( ) const;
void color( const QColor& color );
virtual void mouseDoubleClickEvent( QMouseEvent* me );
signals:
void colorChanged( QColor & col );
private:
QColor c;
};
93 class SpringToolBar : public QToolBar {
Q_OBJECT
public:
SpringToolBar( QMainWindow * parent = 0, const char * name = 0 );
~SpringToolBar( );
//called right after connecting slots;
void init( );
public slots:
void typeChanged( int index );
void newSpringType( );
void ksChanged( float val );
void kdChanged( float val );
void lenChanged( float val );
void colorChanged( QColor& col );
signals:
void typeAltered( SpringType* st );
private:
QComboBox* cb;
ZeroTo500* rl;
ZeroToTen* ks;
ZeroToOne* kd;
QPushButton* newtype;
QLabel *a, *b, *c;
ColorChit * chit;
std::list<SpringType*>* types;
SpringType* current; //currently selected type
int newTypeCount; // for making new names
typedef std::list<SpringType*>::iterator Iter;
void loadDefaults( );
void nameChanged( int pos, const QString& s );
};
/*
class SpringDialog : public QDialog {
Q_OBJECT
public:
SpringDialog( QWidget * parent = 0, const char * name = 0 );
virtual ~SpringDialog( );
};
*/
#endif
1 /****************************************************************************
** $Id: SpringWindow.cpp, v 1.4 2004/09/13 13:50:59 carl Exp $
**
** Copyright ( C ) 1992-2000 Troll Tech AS. All rights reserved.
**
** This file is part of an example program for Qt. This example
** program may be used, distributed and modified without limitation.
**
*****************************************************************************/
#include "SpringWindow.h"
#include <qimage.h>
#include <qpixmap.h>
#include <qtoolbar.h>
#include <qtoolbutton.h>
#include <qpopupmenu.h>
#include <qmenubar.h>
#include <qkeycode.h>
#include <qmultilineedit.h>
#include <qfile.h>
#include <qfiledialog.h>
#include <qstatusbar.h>
#include <qmessagebox.h>
#include <qprinter.h>
#include <qapplication.h>
#include <qaccel.h>
#include <qtextstream.h>
#include <qpainter.h>
#include <qpaintdevicemetrics.h>
#include <qwhatsthis.h>
#include <qinputdialog.h>
#include "matrix/Matrix.h"
#include <config.h>
#define MAGIC_FILE_NUMBER 666666
38 SpringWindow::SpringWindow( )
: QMainWindow( 0, "Mass and Spring Simulation",
WDestructiveClose )
{
int id;
//---- File Menu
QPopupMenu * file = new QPopupMenu( this );
menuBar( )->insertItem( "&File", file );
file->insertItem( "&New", this, SLOT( newDoc( ) ), CTRL+Key_N );
id = file->insertItem( /*openIcon, */ "&Open",
this, SLOT( openMenuAction( ) ), CTRL+Key_O );
id = file->insertItem( /*saveIcon, */ "&Save",
this, SLOT( save( ) ), CTRL+Key_S );
id = file->insertItem( "Save &as...", this, SLOT( saveAs( ) ) );
file->insertSeparator( );
file->insertItem( "&Quit", qApp, SLOT( closeAllWindows( ) ), CTRL+Key_Q );
pf = new ParticleField( this, "Field" );
pf->init( );
pf->setFocus( );
setCentralWidget( pf );
connect( pf, SIGNAL( requestReload( ) ),
this, SLOT( openFile( ) ) );
//---------- Simulation Tool Bar
simTools = new SimToolBar( this, "Sim Controls" );
simTools->setLabel( tr( "Sim Controls" ) );
connect( simTools, SIGNAL( simModeChanged( bool ) ),
pf, SLOT( runSim( bool ) ) );
connect( pf, SIGNAL( simStarted( bool ) ),
simTools, SLOT( setButtonOn( bool ) ) );
connect( simTools, SIGNAL( deltaTChanged( float ) ),
pf, SLOT( setDeltaT( float ) ) );
connect( simTools, SIGNAL( gravityChanged( int ) ),
pf, SLOT( setGravity( int ) ) );
connect( simTools, SIGNAL( wallcoeffChanged( float ) ),
pf, SLOT( setWallRestitution( float ) ) );
connect( simTools, SIGNAL( dragChanged( float ) ),
pf, SLOT( setViscousDrag( float ) ) );
connect( simTools, SIGNAL( updateTimeChanged( int ) ),
pf, SLOT( setUpdateTime( int ) ) );
//-------Spring Tool bar
springTools = new SpringToolBar( this, "Spring Tools" );
connect( springTools, SIGNAL( typeAltered( SpringType* ) ),
pf, SLOT( setSpringType( SpringType* ) ) );
springTools->init( );
//--------Edit Menu
edit = new QPopupMenu( this );
menuBar( )->insertItem( "&Edit", edit );
id = edit->insertItem( "&Box Select",
pf, SLOT( setSelectMode( ) ), CTRL+Key_B );
id = edit->insertItem( "&Make Springs",
pf, SLOT( setSpringMode( ) ), CTRL+Key_M );
connect( edit, SIGNAL( aboutToShow( ) ), this,
SLOT( updateEditMenu( ) ) );
edit->insertSeparator( );
id = edit->insertItem( "Add Constraint...",
this, SLOT( showDialog( ) ) );
edit->insertSeparator( );
id = edit->insertItem( "Snap To Grid",
pf, SLOT( setSnapToGrid( ) ), CTRL+Key_P );
id = edit->insertItem( "Set Grid Spacing...",
this, SLOT( gridSize( ) ) );
id = edit->insertItem( "Clear &All",
pf, SLOT( clearAll( ) ), CTRL+Key_A );
id = edit->insertItem( "Clear Sp&rings",
pf, SLOT( clearSprings( ) ), CTRL+Key_R );
//----- View Menu
view = new QPopupMenu( this );
menuBar( )->insertItem( "&View", view );
id = view->insertItem( "&Trace Particle",
pf, SLOT( toggleTrace( ) ), CTRL+Key_T );
id = view->insertItem( "Clear Trace",
pf, SLOT( clearTrace( ) ), CTRL+Key_C );
view->insertSeparator( );
id = view->insertItem( "Grid",
pf, SLOT( setDisplayGrid( ) ), CTRL+Key_D );
// make the constraint chooser dialog
conDialog = new ConstraintChooser( this, "Make Constraints" );
connect( pf, SIGNAL( numParticlesSelected( unsigned int ) ),
conDialog, SLOT( numParticlesSelected( unsigned int ) ) );
connect( conDialog, SIGNAL( makeConstraint( ConstraintType, float ) ),
pf, SLOT( makeConstraint( ConstraintType, float ) ) );
//----- Menu to bring up Dialog and to change modes
d = new QPopupMenu( this );
menuBar( )->insertItem( "S&im Mode", d );
id = d->insertItem( "Constraint Mode",
pf, SLOT( setCMode( int ) ) );
d->setItemParameter ( id, ( int ) ParticleField::CONSTRAINTS );
id = d->insertItem( "Collisions Mode",
pf, SLOT( setCMode( int ) ) );
d->setItemParameter ( id, ( int ) ParticleField::COLLISIONS );
connect( d, SIGNAL( aboutToShow( ) ), this,
SLOT( updateSimMenu( ) ) );
//------Solver Menu
solvers = new QPopupMenu( this );
menuBar( )->insertItem( "S&olvers", solvers );
id = solvers->insertItem( "Euler Solver",
pf, SLOT( setSolver( int ) ) );
solvers->setItemParameter ( id, ( int ) ParticleField::EULER );
id = solvers->insertItem( "MidPoint Method",
pf, SLOT( setSolver( int ) ) );
solvers->setItemParameter ( id, ( int ) ParticleField::MIDPOINT );
id = solvers->insertItem( "Runge-Kutta-4",
pf, SLOT( setSolver( int ) ) );
solvers->setItemParameter ( id, ( int ) ParticleField::RK );
connect( solvers, SIGNAL( aboutToShow( ) ), this,
SLOT( updateSolverMenu( ) ) );
//------Help Menu
QPopupMenu * help = new QPopupMenu( this );
menuBar( )->insertSeparator( );
menuBar( )->insertItem( "&Help", help );
help->insertItem( "&About", this, SLOT( about( ) ), Key_F1 );
help->insertItem( "About &Qt", this, SLOT( aboutQt( ) ) );
help->insertSeparator( );
connect( pf, SIGNAL( action( char* ) ), this, SLOT( message( char* ) ) );
setCaption( "Springy_Sim - Untitled" );
//make sure we're big enough
adjustSize( ); //doesnt seem to do much
// say that we are done
statusBar( )->message( "Ready", 2000 );
}
193 SpringWindow::~SpringWindow( )
{}
197 void SpringWindow::message( char* s ) {
QString str( s );
statusBar( )->message( s, 2000 );
}
202 void SpringWindow::updateSimMenu( ) {
ParticleField::Mode m = pf->getCMode( );
if( m == ParticleField::CONSTRAINTS ) {
d->setItemChecked( d->idAt( 0 ), true );
d->setItemChecked( d->idAt( 1 ), false );
}
else {
d->setItemChecked( d->idAt( 0 ), false );
d->setItemChecked( d->idAt( 1 ), true );
}
}
216 void SpringWindow::updateEditMenu( ) {
ParticleField::MouseMode m = pf->getMouseMode( );
if( m == ParticleField::SELECT ){
edit->setItemChecked( edit->idAt( 0 ), true );
edit->setItemChecked( edit->idAt( 1 ), false );
}
else {
edit->setItemChecked( edit->idAt( 0 ), false );
edit->setItemChecked( edit->idAt( 1 ), true );
}
}
229 void SpringWindow::updateSolverMenu( ) {
ParticleField::SolverType s = pf->getSolverType( );
if( s == ParticleField::EULER ) {
solvers->setItemChecked( solvers->idAt( 0 ), true );
solvers->setItemChecked( solvers->idAt( 1 ), false );
solvers->setItemChecked( solvers->idAt( 2 ), false );
}
else if ( s == ParticleField::MIDPOINT ){
solvers->setItemChecked( solvers->idAt( 0 ), false );
solvers->setItemChecked( solvers->idAt( 1 ), true );
solvers->setItemChecked( solvers->idAt( 2 ), false );
}
else if ( s == ParticleField::RK ){
solvers->setItemChecked( solvers->idAt( 0 ), false );
solvers->setItemChecked( solvers->idAt( 1 ), false );
solvers->setItemChecked( solvers->idAt( 2 ), true );
}
}
249 void SpringWindow::newDoc( ) {
QString s;
s.sprintf( "newDoc called s" );
statusBar( )->message( s, 2000 );
pf->clearAll( );
setCaption( "Springy_Sim - Untitled" );
}
void
259 SpringWindow::openFile( )
{
QFile f( currentFile );
if( !f.open( IO_ReadOnly ) ) {
statusBar( )->message( "Could not open file", 2000 );
return;
}
QDataStream q( &f );
unsigned int magic = MAGIC_FILE_NUMBER; unsigned int in;
q >>in;
if ( magic != in ) {
statusBar( )->message( "Incorrect file format", 2000 );
return ;
}
pf->load( q );
statusBar( )->message( "file loaded", 2000 );
setCaption( "Springy_Sim - " + truncateFilename( currentFile ) );
SimData sd;
pf->getToolBarSettings( &sd );
simTools->setValues( sd );
}
282 void SpringWindow::openMenuAction( )
{
QString s( QFileDialog::getOpenFileName( QString::null,
"Springy_Sim files ( *.spg )", this ) );
if ( s.isEmpty( ) ) {
statusBar( )->message( "Enter a filename!", 2000 );
return;
}
currentFile = s;
openFile( );
}
296 void SpringWindow::save( )
{
if( currentFile.isNull( ) ) {
saveAs( );
return;
}
saveFile( currentFile );
}
306 void SpringWindow::saveAs( )
{
QString s;
QString fileName = QFileDialog::getSaveFileName( "../saves/newfile.spg",
"Springy_Sim files ( *.spg )", this );
if ( fileName.isNull( ) ) {
s = "You must enter a filename";
statusBar( )->message( s, 2000 );
return;
}
saveFile( fileName );
}
320 bool SpringWindow::saveFile( QString& fileName ){
QString s;
QFile f( fileName );
if( !f.open( IO_WriteOnly ) ) {
s = "Could not open file to write!!";
statusBar( )->message( s, 2000 );
return false;
}
QDataStream q( &f );
unsigned int magic = MAGIC_FILE_NUMBER;
q << magic;
pf->save( q );
f.close( );
statusBar( )->message( "Save complete", 2000 );
currentFile = f.name( );
setCaption( "Springy_Sim - " + truncateFilename( currentFile ) );
return true;
}
343 void SpringWindow::about( )
{
QMessageBox::about( this, "About Springysim",
PACKAGE_STRING " by Carl Lewis\nPlease send bugs, comments and flames to:\n"
PACKAGE_BUGREPORT );
}
351 void SpringWindow::aboutQt( )
{
QMessageBox::aboutQt( this, "Qt Kicks Arse!!" );
}
357 void SpringWindow::showDialog( ) {
conDialog->show( );
}
362 void SpringWindow::gridSize( ) {
bool ok = false;
int res = QInputDialog::getInteger( "Grid Size",
"Please enter a whole number", pf->getGridSpacing( ),
10, 150, 2, &ok, this );
if ( ok ) pf->setGridSpacing( res );
}
372 QString& SpringWindow::truncateFilename( QString& s ){
int pos = s.findRev( '/' );
if ( pos == -1 ) return s;
s.remove( 0, pos+1 );
return s;
}
381 int main( int argc, char **argv ) {
try {
QApplication app( argc, argv );
SpringWindow *s = new SpringWindow( );
s->show( );
app.connect( &app, SIGNAL( lastWindowClosed( ) ), &app, SLOT( quit( ) ) );
return app.exec( );
}
catch( Matrix::Error &e ) {
std::cout<< "Matrix Exception" << std::endl;
std::cout << e.message( ) << std::endl;
}
}
1 /****************************************************************************
** $Id: SpringWindow.h, v 1.3 2004/06/06 08:46:37 carl Exp $
**
** Copyright ( C ) 1992-2000 Troll Tech AS. All rights reserved.
**
** This file is part of an example program for Qt. This example
** program may be used, distributed and modified without limitation.
**
*****************************************************************************/
#ifndef CL_SPRWIN
#define CL_SPRWIN_H
#include <qmainwindow.h>
#include "ParticleField.h"
#include "ConstraintChooser.h"
#include "SimToolBar.h"
#include "SpringType.h"
20 class QMultiLineEdit;
21 class QToolBar;
22 class QPopupMenu;
24 class SpringWindow: public QMainWindow
{
Q_OBJECT
public:
SpringWindow( );
~SpringWindow( );
public slots:
void updateSimMenu( );
void updateEditMenu( );
void updateSolverMenu( );
protected:
//void closeEvent( QCloseEvent* );
private slots:
void save( );
void saveAs( );
void openFile( );
void openMenuAction( );
void newDoc( );
void about( );
void aboutQt( );
void message( char* );
void showDialog( );
void gridSize( );
private:
SimToolBar *simTools;
SpringToolBar *springTools;
ParticleField *pf;
ConstraintChooser* conDialog;
QPopupMenu* d, *edit, *solvers, *view;
QString currentFile;
//void getFileName( );
bool saveFile( QString& fileName );
QString& truncateFilename( QString& s );
};
#endif
1
template<class T>
3 void genDoolittleLU( const Matrix::LMatrix<T> &A,
Matrix::LMatrix<T> &U,
Matrix::LMatrix<T> &L )
{
USHORT j, k, n, s, i;
n = A.numrows( );
T sum;
// calculate elements of U first
for( k = 0 ; k < n ; k++ )
U.setElem( A.getElem( 1, k ), 1, k );
for( j = 1; j < n ; j++ )
for( k = j; k < n; k++ ) {
sum = 0;
for( s=0; s < j-1; s++ )
}
// now find elements of L
}
template<class T>
29 void genCroutLU( const Matrix::LMatrix<T> &A,
Matrix::LMatrix<T> &U,
Matrix::LMatrix<T> &L )
{
}
template<class T>
38 void solveForY( Matrix::LMatrix<T> &L,
Matrix::Vector<T> &b,
Matrix::Vector<T> &y )
{
}
template<class T>
47 void solveForX( Matrix::LMatrix<T> &U,
Matrix::Vector<T> &x,
Matrix::Vector<T> &y )
{
}
template<class T>
56 void LUDoolittleSolve ( Matrix::LMatrix<T> & a, Matrix::Vector<T> & x,
const Matrix::Vector<T> & b ) throw( Matrix::SizeError,
Matrix::SingularMatrix )
{
}
template<class T>
65 void LUDoolittleSolve ( Matrix::LMatrix<T> & a, Matrix::Vector<T> & x,
const Matrix::Vector<T> & b ) throw( Matrix::SizeError,
Matrix::SingularMatrix )
{
}
1
/*
* LUSolve: Functions for solving matrix equations with LU factorisation
* Formulae taken from Erwin Kreysig, Advanced Engineering Mathematics,
* 7th Ed., p981
*/
#ifndef SPRINGY_SIM_LUSOLVE_H
#define SPRINGY_SIM_LUSOLVE_H
template<class T>
12 void genDoolittleLU( const Matrix::LMatrix<T> &A,
Matrix::LMatrix<T> &U,
Matrix::LMatrix<T> &L );
template<class T>
18 void genCroutLU( const Matrix::LMatrix<T> &A,
Matrix::LMatrix<T> &U,
Matrix::LMatrix<T> &L );
template<class T>
24 void solveForY( Matrix::LMatrix<T> &L,
Matrix::Vector<T> &b,
Matrix::Vector<T> &y );
template<class T>
30 void solveForX( Matrix::LMatrix<T> &U,
Matrix::Vector<T> &x,
Matrix::Vector<T> &y );
template<class T>
36 void LUDoolittleSolve ( Matrix::LMatrix<T> & a, Matrix::Vector<T> & x,
const Matrix::Vector<T> & b ) throw( Matrix::SizeError,
Matrix::SingularMatrix );
template<class T>
42 void LUDoolittleSolve ( Matrix::LMatrix<T> & a, Matrix::Vector<T> & x,
const Matrix::Vector<T> & b ) throw( Matrix::SizeError,
Matrix::SingularMatrix );
#include "LUSolve.cpp"
#endif // SPRINGY_SIM_LUSOLVE_H
template<class T>
8 bool Matrix::Base<T>::operator==( Base& m ) const {
if( ! sameShape( m ) ) return false;
for ( USHORT i = 0; i < numrows( ); i++ ) {
for ( USHORT j = 0; j < numcols( ); j++ ) {
if ( getElem( i, j ) != m.getElem( i, j ) )
return false;
}
}
return true;
}
template<class T>
21 bool Matrix::Base<T>::sameShape( const Base & m ) const {
if( numrows( ) != m.numrows( ) ) return false;
if( numcols( ) != m.numcols( ) ) return false;
return true;
}
////////////////////////////////////////////
// Matrix::LMatrix<T>::Inner<T> Member functions
////////////////////////////////////////////
// basic constructor
template<class T>
35 Matrix::LMatrix<T>::Inner::Inner( USHORT i, USHORT j ) {
rows = i; cols = j;
numrefs = 1;
data = ( T* ) new char[sizeof( T ) * i * j];
}
// copy constructor - makes a deep copy of the
// Inner
template<class T>
44 Matrix::LMatrix<T>::Inner::Inner( Inner & inner ) {
numrefs = 1;
rows = inner.rows; cols = inner.cols;
USHORT max = rows * cols;
data = ( T* ) new char[sizeof( T ) * max];
T* p = data; T* q = inner.data;
for ( ; p < ( data + max ); p++, q++ ) *p = *q;
}
//destructor
template<class T>
56 Matrix::LMatrix<T>::Inner::~Inner( ) {
delete [] data;
}
template<class T>
62 inline void Matrix::LMatrix<T>::Inner::attach( ) {
numrefs++;
}
template<class T>
68 void Matrix::LMatrix<T>::Inner::detach( ) {
if ( --numrefs == 0 ) delete this;
}
template<class T>
74 inline USHORT Matrix::LMatrix<T>::Inner::numrows( ) {
return rows;
}
template<class T>
80 inline USHORT Matrix::LMatrix<T>::Inner::numcols( ) {
return cols;
}
template<class T>
typename Matrix::LMatrix<T>::Inner*
Matrix::LMatrix<T>::Inner::unalias( ) {
87 if( numrefs == 1 ) return this;
88 return new Inner( *this );
}
///////////////////////////////////////////
// LMatrix Member functions
///////////////////////////////////////////
// standard constructor
template<class T>
Matrix::LMatrix<T>::LMatrix( USHORT numrows, USHORT numcols ){
n = new Inner( numrows, numcols );
}
// copy constructor
template<class T>
Matrix::LMatrix<T>::LMatrix( const LMatrix & m ) {
n = m.n;
n->attach( );
}
//assignment operators
template<class T>
Matrix::LMatrix<T> &
Matrix::LMatrix<T>::operator=( const LMatrix & m ) {
if( ! ( &m == this ) ) {
n->detach( );
n = m.n;
n->attach( );
}
return *this;
}
/*template<class T>
Matrix::Base<T> &
Matrix::LMatrix<T>::operator=( Base & b ) {
if ( !( &b == this ) ) {
LMatrix<T>* p = 0; Base<T> * bp = &b;
p = dynamic_cast<LMatrix<T> *>( bp );
if( p ) this->operator=( *p );
else {
USHORT r, c;
n = new Inner( r = b.numrows( ), c = b.numcols( ) );
for ( USHORT i = 0; i < r ; i++ )
for ( USHORT j = 0; j < c ; j++ )
n->data[i*c +j] = b.getElem( );
}
}
return *this;
} */
//destructor
template<class T>
Matrix::LMatrix<T>::~LMatrix( ) {
n->detach( );
}
template<class T>
USHORT Matrix::LMatrix<T>::numrows( ) const {
return n->numrows( );
}
template<class T>
USHORT Matrix::LMatrix<T>::numcols( ) const {
return n->numcols( );
}
template<class T>
T Matrix::LMatrix<T>::getElem( USHORT i, USHORT j ) const
#ifdef MX_BOUNDS_CHK
throw( IndexError )
#endif
{
#ifdef MX_BOUNDS_CHK
if( ( i>= n->numrows( ) ) || ( j>= n->numcols( ) ) )
throw IndexError( );
#endif
return n->data[i*( n->numcols( ) ) + j];
}
template<class T>
void Matrix::LMatrix<T>::transpose( ) {
Inner* old = n;
USHORT orows, ocols;
n = new Inner( ocols = n->numcols( ), orows = old->numrows( ) );
for ( USHORT i = 0; i < orows; i++ )
for ( USHORT j = 0; j < ocols; j++ )
n->data[j*orows+ i] = old->data[i*ocols + j];
old->detach( );
}
template<class T>
void Matrix::LMatrix<T>::setElem( T val, USHORT i, USHORT j )
#ifdef MX_BOUNDS_CHK
throw( IndexError )
#endif
{
#ifdef MX_BOUNDS_CHK
if( ( i>= n->numrows( ) ) || ( j>= n->numcols( ) ) )
throw IndexError( );
#endif
n = n->unalias( );
n->data[i*( n->numcols( ) )+ j] = val;
}
template<class T>
void Matrix::LMatrix<T>::zero( ) {
n = n->unalias( );
USHORT max = n->numrows( ) * ( n->numcols( ) );
T* p = n->data;
for( ; p < ( n->data + max );p++ ) *p = 0;
}
// scalar multiplication
template<class T>
Matrix::LMatrix<T>
Matrix::LMatrix<T>::operator*( const T &t ) const {
USHORT rows, cols, max;
LMatrix<T> ret( rows = n->numrows( ), cols = n->numcols( ) );
T* p = n->data; T* q = ret.n->data;
max = rows*cols;
for( ; p < ( n->data + max );p++, q++ ) ( *q ) = ( *p ) * t;
return ret;
}
///////////////////////////////////
// Diagonal Member functions
///////////////////////////////////
//constructor
template<class T>
Matrix::Diagonal<T>::Diagonal( USHORT size ) :
_size( size ) {
data = new T[_size];
}
// copy constructor
// no lazy copy
template<class T>
Matrix::Diagonal<T>::Diagonal( const Diagonal & d ) {
_size = d._size;
data = new T[_size];
for ( USHORT i = 0; i < _size; i++ )
data[i] = d.data[i];
}
// assignment operator
template<class T>
Matrix::Diagonal<T> &
Matrix::Diagonal<T>::operator=( const Diagonal& d ) {
if( &d != this ) {
if ( _size != d._size ){
delete [] data;
_size = d._size;
data = new T[_size];
}
for ( USHORT i = 0; i < _size; i++ )
data[i] = d.data[i];
}
return *this;
}
// destructor
template<class T>
Matrix::Diagonal<T>::~Diagonal( ) {
delete [] data;
}
template<class T>
void Matrix::Diagonal<T>::setElem( T val, USHORT i, USHORT j )
#ifdef MX_BOUNDS_CHK
throw( IndexError )
#endif
{
#ifdef MX_BOUNDS_CHK
if( i >= _size || j >= _size || i != j )
throw IndexError( );
#endif
data[i] = val;
}
template<class T>
T Matrix::Diagonal<T>::getElem( USHORT i, USHORT j ) const
#ifdef MX_BOUNDS_CHK
throw( IndexError )
#endif
{
#ifdef MX_BOUNDS_CHK
if( i >= _size || j >= _size )
throw IndexError( );
#endif
if ( i != j ) return 0;
return data[i];
}
template<class T>
void Matrix::Diagonal<T>::zero( ) {
for( USHORT i = 0; i < _size; i++ ) {
data[i] = 0;
}
}
// scalar multiplication
template<class T>
Matrix::Diagonal<T>
Matrix::Diagonal<T>::operator*( const T& t ) const{
Diagonal<T> d( _size );
for( USHORT i = 0; i < _size; i++ )
d.data[i] = t * ( data[i] );
return d;
}
template<class T>
void Matrix::Diagonal<T>::identify( ) {
for( USHORT i = 0; i < _size; i++ )
data[i] = 1.0;
}
/////////////////////////////////////////
// Vector Methods
////////////////////////////////////////
template<class T>
Matrix::Vector<T>::Inner::Inner( USHORT i ) {
length = i;
data = new T[i];
numrefs = 1;
}
template<class T>
Matrix::Vector<T>::Inner::Inner( const Inner & n ) {
length = 0;
data = 0;
numrefs = 1;
this->operator=( n );
}
template<class T>
typename Matrix::Vector<T>::Inner &
Matrix::Vector<T>::Inner::operator=( const Inner & n ) {
if( this != & n ) {
if( length != n.length ) {
length = n.length;
delete [] data;
data = new T[length];
}
for ( USHORT i = 0; i < length; i++ )
data[i] = n.data[i];
}
return *this;
}
template<class T>
Matrix::Vector<T>::Inner::~Inner( ) {
delete [] data;
}
template<class T>
inline void
Matrix::Vector<T>::Inner::attach( ) {
++numrefs;
}
template<class T>
void
Matrix::Vector<T>::Inner::detach( ) {
if( --numrefs == 0 ) delete this;
}
template<class T>
typename Matrix::Vector<T>::Inner*
Matrix::Vector<T>::Inner::unalias( ) {
if ( numrefs == 1 ) return this;
return new Inner( *this );
}
template<class T>
Matrix::Vector<T>::Vector( USHORT length ) {
n = new Inner( length );
}
template<class T>
Matrix::Vector<T>::Vector( const Vector & v ) {
n = v.n;
n->attach( );
}
template<class T>
Matrix::Vector<T> &
Matrix::Vector<T>::operator=( const Vector & v ) {
if( this != &v ) {
n->detach( );
n= v.n;
n->attach( );
}
return *this;
}
template<class T>
Matrix::Vector<T>::~Vector( ) {
n->detach( );
}
template<class T>
USHORT
Matrix::Vector<T>::length( ) const {
return n->length;
}
template<class T>
void
Matrix::Vector<T>::init( T value ) {
n = n->unalias( );
for ( int i = 0; i < n->length; i++ )
n->data[i] = value;
}
template<class T>
void
Matrix::Vector<T>::setElem( const T & val, USHORT index )
#ifdef MX_BOUNDS_CHK
throw( Matrix::IndexError )
#endif
{
#ifdef MX_BOUNDS_CHK
if( index >= n->length ) throw IndexError( );
#endif
n = n->unalias( );
n->data[index] = val;
}
template<class T>
T
Matrix::Vector<T>::getElem( USHORT index ) const
#ifdef MX_BOUNDS_CHK
throw( Matrix::IndexError )
#endif
{
#ifdef MX_BOUNDS_CHK
if( index >= n->length ) throw IndexError( );
#endif
return n->data[index];
}
template<class T>
T
Matrix::Vector<T>::dotprod( const Vector & v ) const
#ifdef MX_BOUNDS_CHK
throw( Matrix::SizeError )
#endif
{
#ifdef MX_BOUNDS_CHK
if( n->length != v.n->length ) throw SizeError( );
#endif
T res = 0;
for( USHORT i = 0; i < n->length; i++ )
res += ( n->data[i] * ( v.n->data[i] ) );
return res;
}
template<class T>
Matrix::Vector<T>
Matrix::Vector<T>::operator+( const Matrix::Vector<T> & v ) const
throw ( Matrix::SizeError ) {
if ( n->length != v.n->length ) throw Matrix::SizeError( );
Matrix::Vector<T> ret( n->length );
for ( USHORT i = 0; i < n->length; i++ ) {
ret.n->data[i] = n->data[i] + v.n->data[i];
}
return ret;
}
template<class T>
Matrix::Vector<T>
Matrix::Vector<T>::operator-( const Matrix::Vector<T> & v ) const
throw ( Matrix::SizeError ) {
if ( n->length != v.n->length ) throw Matrix::SizeError( );
Matrix::Vector<T> ret( n->length );
for ( USHORT i = 0; i < n->length; i++ ) {
ret.n->data[i] = n->data[i] - v.n->data[i];
}
return ret;
}
/////////////////////////////////////////
// Global Functions
/////////////////////////////////////////
/*ostream& operator<< ( ostream & os, Matrix::Error & e ) {
string s = e.message( );
os << s;
return os;
} */
/////////////////////////////////////
// Matrix Addition
template<class T>
Matrix::LMatrix<T>
operator+( const Matrix::Base<T> & bl,
const Matrix::Base<T> & br )
throw( Matrix::SizeError ) {
if( !bl.sameShape( br ) ) throw Matrix::SizeError( );
USHORT rows, cols;
Matrix::LMatrix<T> ret( rows = bl.numrows( ), cols = bl.numcols( ) );
for ( USHORT i = 0; i < rows; i++ )
for ( USHORT j = 0; j < cols; j++ )
ret.n->data[i*cols+j] =
bl.getElem( i, j ) + br.getElem( i, j );
return ret;
}
// takes two LMatrix's instad.
// may be slightly faster
template<class T>
Matrix::LMatrix<T>
operator+( const Matrix::LMatrix<T> & bl,
const Matrix::LMatrix<T> & br )
throw( Matrix::SizeError ) {
if( !bl.sameShape( br ) ) throw Matrix::SizeError( );
USHORT rows, cols, max;
Matrix::LMatrix<T> ret( rows = bl.numrows( ), cols = bl.numcols( ) );
max = rows*cols;
for ( USHORT i = 0; i < max; i++ )
ret.n->data[i] =
bl.n->data[i] + br.n->data[i];
return ret;
}
/////////////////////////////////////
// Matrix multiplication
template<class T>
Matrix::LMatrix<T>
operator*( const Matrix::Base<T> & bl, const Matrix::Base<T> &br )
throw( Matrix::SizeError ) {
USHORT brows;
if( bl.numcols( ) != ( brows = br.numrows( ) ) ) throw Matrix::SizeError( );
USHORT newrows, newcols, s;
T sum;
s = bl.numcols( );
Matrix::LMatrix<T> ret( newrows = bl.numrows( ), newcols = br.numcols( ) );
for ( USHORT i = 0; i < newrows; i++ )
for ( USHORT j = 0; j < newcols; j++ ) {
sum = 0;
for ( USHORT k = 0; k < s; k++ ) {
sum += bl.getElem( i, k ) * br.getElem( k, j );
}
ret.n->data[i*newrows+j] = sum;
}
return ret;
}
template<class T>
Matrix::LMatrix<T>
operator*( const Matrix::LMatrix<T> & ml, const Matrix::LMatrix<T> &mr )
throw( Matrix::SizeError ) {
USHORT ml_rows = ml.n->numrows( );
USHORT ml_cols = ml.n->numcols( );
USHORT mr_rows = mr.n->numrows( );
USHORT mr_cols = mr.n->numcols( );
if( ml_cols != mr_rows ) throw Matrix::SizeError( );
T sum;
Matrix::LMatrix<T> ret( ml_rows, mr_cols );
for ( USHORT i = 0; i < ml_rows; i++ )
for ( USHORT j = 0; j < mr_cols; j++ ) {
sum = 0;
for ( USHORT k = 0; k < ml_cols; k++ ) {
sum += ml.n->data[i*ml_cols + k] *
mr.n->data[k*mr_cols + j];
}
ret.n->data[i*ml_rows+j] = sum;
}
return ret;
}
// multiply Diagonal ( on left ) with LMatrix
template<class T>
Matrix::LMatrix<T> operator*
( const Matrix::Diagonal<T> & dl, const Matrix::LMatrix<T> & mr )
throw( Matrix::SizeError ) {
//cout << "diag mult" << endl;
USHORT dl_size = dl._size;
USHORT mr_rows = mr.n->numrows( );
USHORT mr_cols = mr.n->numcols( );
USHORT index = 0;
if( dl_size != mr_rows ) throw Matrix::SizeError( );
Matrix::LMatrix<T> ret( dl_size, mr_cols );
for ( USHORT i = 0; i < dl_size; i++ ) {
for ( USHORT j = 0; j < mr_cols; j++ ) {
ret.n->data[index+j] = dl.data[i] * mr.n->data[index+j];
}
index += mr_cols;
}
return ret;
}
// multiply Diagonal ( on right ) with LMatrix
template<class T>
Matrix::LMatrix<T> operator*
( const Matrix::LMatrix<T> & ml, const Matrix::Diagonal<T> & dr )
throw( Matrix::SizeError ){
//cout << "diag mult" << endl;
USHORT ml_rows = ml.n->numrows( );
USHORT ml_cols = ml.n->numcols( );
USHORT dr_size = dr._size;
USHORT index = 0;
T mult;
if( ml_cols != dr_size ) throw Matrix::SizeError( );
Matrix::LMatrix<T> ret( ml_rows, dr_size );
for ( USHORT i = 0; i < ml_cols; i++ ) {
mult = dr.data[i]; index = 0;
for ( USHORT j = 0; j < ml_rows; j++ ) {
ret.n->data[index+i] = mult * ml.n->data[index+i];
index += ml_cols;
}
}
return ret;
}
template<class T>
Matrix::Vector<T>
operator*( const T & t, const Matrix::Vector<T> & v ) {
Matrix::Vector<T> res( v.n->length );
for ( int i = 0; i < v.n->length; i++ )
res.n->data[i] = t* ( v.n->data[i] );
return res;
}
template<class T>
Matrix::Vector<T>
operator*( const Matrix::Vector<T> & v, const T & t ) {
Matrix::Vector<T> res( v.n->length );
for ( int i = 0; i < v.n->length; i++ )
res.n->data[i] = t* ( v.n->data[i] );
return res;
}
template<class T>
Matrix::Vector<T> operator*
( const Matrix::Vector<T> & vl, const Matrix::LMatrix<T> & mr )
throw( Matrix::SizeError ){
USHORT mr_rows, mr_cols;
if( vl.length( ) != ( mr_rows = mr.numrows( ) ) )
throw Matrix::SizeError( "operator*: Vector x LMatrix" );
T sum;
Matrix::Vector<T> ret( mr_cols = mr.numcols( ) );
for ( USHORT i = 0; i < mr_cols;i++ ) {
sum = 0;
for ( USHORT j = 0; j < mr_rows; j++ ) {
sum += ( vl.n->data[j] ) * ( mr.n->data[j*mr_cols+i] );
}
ret.n->data[i] = sum;
}
return ret;
}
template<class T>
Matrix::Vector<T> operator*
( const Matrix::LMatrix<T> & ml, const Matrix::Vector<T> & vr )
throw( Matrix::SizeError ){
USHORT ml_rows, ml_cols;
if( vr.length( ) != ( ml_cols = ml.numcols( ) ) )
throw Matrix::SizeError( "operator*: LMatrix x Vector" );
T sum;
Matrix::Vector<T> ret( ml_rows = ml.numrows( ) );
for ( USHORT i = 0; i < ml_rows;i++ ) {
sum = 0;
for ( USHORT j = 0; j < ml_cols; j++ ) {
sum += ( vr.n->data[j] ) * ( ml.n->data[i*ml_cols+j] );
}
ret.n->data[i] = sum;
}
return ret;
}
template<class T>
void ChSolve( Matrix::LMatrix<T> & a, Matrix::Vector<T> & x,
const Matrix::Vector<T> &b ) throw( Matrix::SizeError,
Matrix::SingularMatrix )
{
static T p[256];
T sum;
USHORT n = a.numrows( );
USHORT k;
if( n > 255 )
throw Matrix::SizeError( "ChSolve: Matrix too large" );
if( n != a.numcols( ) )
throw Matrix::SizeError( "ChSolve: non-square Matrix" );
if( n != b.length( ) || n != x.length( ) )
throw Matrix::SizeError( "ChSolve: bad vector length" );
for( USHORT i = 0; i < n; i++ )
for( USHORT j = i; j < n; j++ ) {
for( sum = a.getElem( i, j ), k = i - 1; k < 255; k-- )
sum -= a.getElem( i, k ) * a.getElem( j, k );
if( i == j ) {
if( sum <= 0 ) throw Matrix::SingularMatrix( );
p[i] = sqrt( sum );
}
else a.setElem( ( sum / p[i] ), j, i );
}
for( USHORT i = 0; i < n; i++ )
{
for( sum = b.getElem( i ), k = i - 1; k < 255; k-- )
sum -= a.getElem( i, k ) * x.getElem( k );
x.setElem( ( sum / p[i] ), i );
}
for( USHORT i = n - 1; i < 257; i-- )
{
for( sum = x.getElem( i ), k = i + 1; k < n; k++ )
sum -= a.getElem( k, i ) * x.getElem( k );
x.setElem( ( sum/p[i] ) , i );
}
}
#ifndef cl_matrix
#define cl_matrix
#include <iostream>
#include <string>
#include <math.h>
#define MX_BOUNDS_CHK
typedef unsigned short int USHORT;
namespace Matrix {
template<class T>
15 class LMatrix;
template<class T>
17 class Diagonal;
template<class T>
20 class Vector;
22 class Error;
23 class SizeError;
//exceptions
26 class Error{
protected:
28 std::string* m;
public:
30 Error( ){m= 0;}
31 Error( char* s ){ m = new std::string( s ); }
32 virtual ~Error( ){ delete m; }
33 virtual const std::string message( ) const {
if( m != 0 ) return *m;
else return "no info";
}
};
39 class SizeError : public Error {
public:
41 SizeError( ): Error( ) {
name = new std::string( "Matrix::SizeError" );
}
44 SizeError( char* s ):
Error( s ) {}
46 const std::string message( ) const {
if( m != 0 ) return *m;
else return *name;
}
private:
52 std::string* name;
};
55 class IndexError : public Error {
public:
57 IndexError( ) : Error( ) {
name = new std::string( "Matrix::IndexError" );
}
60 IndexError( char*s ):
Error( s ){}
62 const std::string message( ) const {
if( m != 0 ) return *m;
else return *name;
}
private:
68 std::string* name;
};
71 class BlockError : public Error{};
72 class ReadOnlyError : public Error{};
73 class NotYetImplemented : public Error{};
74 class SingularMatrix : public Error{
public:
76 SingularMatrix( ): Error( ) {
name = new std::string( "Matrix::SingularMatrix" );
}
79 SingularMatrix( char* s ):
Error( s ) {}
81 const std::string message( ) const {
if( m != 0 ) return *m;
else return *name;
}
private:
86 std::string* name;
};
} // namespace Matrix
/////////////////////////////////////
// Global friends -
// forward declarations required
// add two LMatrix's
template<class T>
97 Matrix::LMatrix<T> operator+
( const Matrix::LMatrix<T> & lm, const Matrix::LMatrix<T> & rm )
99 throw( Matrix::SizeError );
// multiply two LMatrix's
template<class T>
103 Matrix::LMatrix<T> operator*
( const Matrix::LMatrix<T> & bl, const Matrix::LMatrix<T> & br )
105 throw( Matrix::SizeError );
// multiply Diagonal ( on left ) with LMatrix
template<class T>
109 Matrix::LMatrix<T> operator*
( const Matrix::Diagonal<T> & dl, const Matrix::LMatrix<T> & mr )
111 throw( Matrix::SizeError );
// multiply Diagonal ( on right ) with LMatrix
template<class T>
115 Matrix::LMatrix<T> operator*
( const Matrix::LMatrix<T> & ml, const Matrix::Diagonal<T> & dr )
117 throw( Matrix::SizeError );
/////////////////////////////////////
// Classes
namespace Matrix {
//abstract base class for all matrices.
template<class T>
131 class Base {
public:
133 Base( ){}
134 Base( Base & m ){}
//virtual Base & operator=( Base & rhs )= 0 ;
136 virtual ~Base( ){}
137 virtual bool operator==( Base & m ) const;
138 virtual void zero( ) = 0;
//class M_proxy{};
//M_proxy<T> operator[]( int i );
143 virtual void transpose( ) = 0;
145 virtual USHORT numrows( ) const = 0;
146 virtual USHORT numcols( ) const = 0;
147 virtual bool sameShape( const Base & m ) const;
virtual T getElem( USHORT i, USHORT j ) const
#ifdef MX_BOUNDS_CHK
throw( IndexError )
#endif
= 0;
};
template<class T>
class Writable : public Base<T> {
public:
/*virtual Base & operator+=( Base & m ) = 0;
virtual Base & operator*=( Base & m ) = 0;
virtual Base & operator*=( T& t ) = 0; */
virtual void setElem( T val, USHORT i, USHORT j )
#ifdef MX_BOUNDS_CHK
throw( IndexError )
#endif
= 0;
};
// Literal matrix: exactly the same interface
// as Matrix. Stores values as an array.
template<class T>
class LMatrix : public Writable<T> {
public:
LMatrix( USHORT numrows, USHORT numcols );
LMatrix( const LMatrix & m );
LMatrix & operator=( const LMatrix & b );
//virtual Base & operator=( Base & b );
virtual ~LMatrix( );
virtual USHORT numrows( ) const;
virtual USHORT numcols( ) const;
virtual void transpose( );
LMatrix operator*( const T & t ) const;
void setElem( T val, USHORT i, USHORT j )
#ifdef MX_BOUNDS_CHK
throw( IndexError );
#endif
T getElem( USHORT i, USHORT j ) const
#ifdef MX_BOUNDS_CHK
throw( IndexError );
#endif
void zero( );
/* // friends
friend LMatrix operator+<T>( const LMatrix<T> & bl,
const LMatrix<T> & br )
throw( SizeError );
friend LMatrix operator*<T>( const LMatrix<T> & bl,
const LMatrix<T> & br )
throw( SizeError );
// multiply Diagonal ( on left ) with LMatrix
friend LMatrix<T> operator*<T>
( const Diagonal<T> & dl, const LMatrix<T> & br )
throw( SizeError );
// multiply Diagonal ( on right ) with LMatrix
friend LMatrix<T> operator*<T>
( const LMatrix<T> & bl, const Diagonal<T> & dr )
throw( SizeError );
friend Matrix::Vector<T>
operator*<T> ( const Matrix::Vector<T> & vl, const Matrix::LMatrix<T> & mr )
throw( Matrix::SizeError );
friend Matrix::Vector<T>
operator*<T> ( const LMatrix<T> & ml, const Vector<T> & vr )
throw( Matrix::SizeError ); */
class Inner{
public:
Inner( USHORT i, USHORT j );
Inner( Inner & inner );
~Inner( );
T* data;
void attach( );
void detach( );
USHORT numrows( );
USHORT numcols( );
Inner* unalias( );
private:
USHORT numrefs;
USHORT rows;
USHORT cols;
};
public:
Inner* n;
};
// Sparse matrix - holds values in linked lists
// All values not explicitly set are zero.
// Same interface as matrix
template<class T>
class SMatrix : public Writable<T> {
};
//wraps some other data structure up into
// a matrix or vector. gets its data from
// some other object of class S.
// only supports read-only operations.
template<class T, class S>
class PMatrix : public Base<T> {
};
// This class is used for diagonal matrices
// all entries not on the main diagonal are zero
// Forgetting it is strictly diagonal will probably
// mean an exception gets thrown
// diagonal matrices are always square
// no lazy copying at this stage.
template<class T>
class Diagonal : public Base<T> {
public:
explicit Diagonal( USHORT size );
Diagonal( const Diagonal& d );
Diagonal& operator=( const Diagonal & d );
~Diagonal( );
virtual USHORT numrows( ) const { return _size; }
virtual USHORT numcols( ) const { return _size; }
virtual void transpose( ){}
void setElem( T val, USHORT i, USHORT j )
#ifdef MX_BOUNDS_CHK
throw( IndexError )
#endif
;
T getElem( USHORT i, USHORT j ) const
#ifdef MX_BOUNDS_CHK
throw( IndexError )
#endif
;
void zero( );
Diagonal operator*( const T & t ) const;
// multiply Diagonal ( on left ) with LMatrix
//template<class T> friend LMatrix<T> operator* < T > ( const Diagonal<T> & dl, const LMatrix<T> & mr )
// throw( SizeError );
//multiply Diagonal ( on right ) with LMatrix
//friend LMatrix<T> operator*<T>
//( const LMatrix<T> & ml, const Diagonal<T> & dr )
//throw( SizeError );
void identify( );
private:
T* data;
USHORT _size;
};
template<class T>
class Vector {
public:
explicit Vector( USHORT length );
Vector( const Vector & v );
Vector& operator=( const Vector & v );
~Vector( );
USHORT length( ) const ;
void init( T value );
void setElem( const T & val, USHORT index )
#ifdef MX_BOUNDS_CHK
throw( IndexError );
#endif
;
T getElem( USHORT index ) const
#ifdef MX_BOUNDS_CHK
throw( IndexError );
#endif
;
T dotprod( const Vector & v ) const
#ifdef MX_BOUNDS_CHK
throw( SizeError );
#endif
;
/* friend Matrix::Vector<T>
operator*<T>( const T & t, const Matrix::Vector<T> & v );
friend Matrix::Vector<T>
operator*<T>( const Matrix::Vector<T> & v, const T & t );
friend Matrix::Vector<T>
operator*<T> ( const Matrix::Vector<T> & vl, const Matrix::LMatrix<T> & mr )
throw( Matrix::SizeError );
friend Matrix::Vector<T>
operator*<T> ( const Matrix::LMatrix<T> & ml, const Matrix::Vector<T> & vr )
throw( Matrix::SizeError );*/
Vector operator+( const Vector& v ) const throw( SizeError );
Vector operator-( const Vector& v ) const throw( SizeError );
//private:
class Inner {
public:
Inner( USHORT i );
Inner( const Inner & n );
Inner& operator=( const Inner & n );
~Inner( );
T* data;
void attach( );
void detach( );
Inner* unalias( );
USHORT numrefs;
USHORT length;
};
Inner* n;
};
} //namespace Matrix
///////////////////////////////////////
// Global Functions
///////////////////////////////////////
//ostream& operator<< ( ostream & os, Matrix::Error & e );
///////////////////////////////////////
// utility routines to make scalar multiplication
// 'commutative'
template<class T>
Matrix::Diagonal<T> operator*( const T& t,
const Matrix::Diagonal<T> & d ) {
return d.operator*( t );
}
template<class T>
Matrix::LMatrix<T> operator*( const T & t,
const Matrix::LMatrix<T> & m ) {
return m.operator*( t );
}
/////////////////////////////////////////
// Matrix Addition routines
// add two arbitary matrices
template<class T>
Matrix::LMatrix<T> operator+
( const Matrix::Base<T> & bl, const Matrix::Base<T> & br )
throw( Matrix::SizeError );
////////////////////////////////////////
// Matrix Multiplication routines
// multiply any two matrices,
// get an LMatrix back
template<class T>
Matrix::LMatrix<T> operator*
( const Matrix::Base<T> & bl, const Matrix::Base<T> & br )
throw( Matrix::SizeError );
///////////////////////////////////////
// Vector x Scalar routines
template<class T>
Matrix::Vector<T>
operator*( const T & t, const Matrix::Vector<T> & v );
template<class T>
Matrix::Vector<T>
operator*( const Matrix::Vector<T> & v, const T & t );
///////////////////////////////////////
// Vector x LMatrix Routines
template<class T>
Matrix::Vector<T> operator*
( const Matrix::Vector<T> & vl, const Matrix::LMatrix<T> & mr )
throw( Matrix::SizeError );
template<class T>
Matrix::Vector<T> operator*
( const Matrix::LMatrix<T> & ml, const Matrix::Vector<T> & vr )
throw( Matrix::SizeError );
////////////////////////////////////////
// Matrix eqn solving routine
// Code not wholly original
// assumes the matrix is symmetric positive definite
///////////////////////////////////////
template<class T>
void ChSolve ( Matrix::LMatrix<T> & a, Matrix::Vector<T> & x,
const Matrix::Vector<T> & b ) throw( Matrix::SizeError,
Matrix::SingularMatrix );
#include "Matrix.cpp"
////const string Matrix::SizeError::name( "Matrix::SizeError" );
//const string Matrix::IndexError::name( "Matrix::IndexError" );
//const string Matrix::SingularMatrix::name( "Matrix::SingularMatrix" );
#endif
1 #include "Matrix.h"
# define PR( x ) ( cout << #x << ":\n "<< ( x ) << endl )
4 int main( ) {
Matrix::Vector<float> a( 4 );
Matrix::Vector<float> b( 4 );
a.init( 1.0 );
b.init( 2.0 );
PR( a );
PR( b );
cout <<"c = a + b" << endl;
Matrix::Vector<float> c = a + b;
PR( c );
c = c - ( 2.0f*b );
cout <<"c = c - ( 2.0f*b )" << endl;
PR( c );
1
template<class T>
5 std::ostream& operator<<( std::ostream& os, Matrix::Base<T> & b ) {
for ( USHORT i = 0; i < b.numrows( ); i++ ) {
for ( USHORT j = 0; j < b.numcols( ); j++ ) {
os.width( os.precision( ) + 3 );
os << b.getElem( i, j );
}
os << endl;
}
return os;
}
template<class T>
17 std::ostream& operator<<( std::ostream & os, Matrix::Vector<T> & v ) {
for ( USHORT i = 0; i < v.length( ); i++ ) {
os.width( os.precision( ) + 3 );
os << v.getElem( i );
}
return os;
}
1 #ifndef cl_io_matrix
#define cl_io_matrix
#include <iostream>
#include "Matrix.h"
template< class T>
7 std::ostream& operator<<( std::ostream & os, Matrix::Base<T> & b );
template< class T>
10 std::ostream& operator<<( std::ostream & os, Matrix::Vector<T> & v );
#include "matrix_io.cpp"
#endif