void BHTree::calculateForce(BHTree& node, Body& b, bool canCombineMass)
{
if(!node.isInternalNode) {
if(node.hasBody && node.body != b) {
//apply force of current node to body
applyForceToBody(b, node.body, canCombineMass);
}
}
else
{
float s = node.width;
float d = Body::distance(node.cgX, node.cgY, node.cgZ, b.x, b.y, b.z);
float ratio = s/d;
if(s/d <= BH_THETA) {
//apply force of current node to body
applyForceToBody(b, Body(-1, node.mass, node.cgX, node.cgY, node.cgZ), false);
}
else
{
//recursively apply force of child nodes
calculateForce(*node.fNW, b, canCombineMass);
calculateForce(*node.fNE, b, canCombineMass);
calculateForce(*node.fSW, b, canCombineMass);
calculateForce(*node.fSE, b, canCombineMass);
calculateForce(*node.bNW, b, canCombineMass);
calculateForce(*node.bNE, b, canCombineMass);
calculateForce(*node.bSW, b, canCombineMass);
calculateForce(*node.bSE, b, canCombineMass);
}
}
}
void nbodyBarnesHut(particle * particles,
int count,
int n,
double timeDiff,
double eps,
char * outputPath)
{
int step;
sprintf(fileParamBuff, "%s_%%0%dd.vtk", outputPath, ((int) log10(n) + 1));
bodyptr bodies = convertStruct(particles, count);
/* End converting */
/* Do the stepping */
bodyptr p;
for (step = 1; step <= 1; step++) {
cellptr root;
real rootSize;
/* Make the tree */
treeInit(&root, bodies, count, &rootSize);
/* Calculate force */
calculateForce(root, eps, rootSize);
/* Advance each body */
for (p = bodies; p < bodies + count; p++) {
/* Advance the speed */
/* Advance the position */
}
/* Free the tree */
treeFree(&root);
}
}
int main()
{
std::ifstream scienceData("science.txt", std::ios::in);
if (!scienceData) {
std::cout << "can not open file" << std::endl;
return 1;
}
double velocity;
double delta_time;
double acceleration;
double mass;
double force;
while (scienceData >> velocity >> delta_time >> mass) {
acceleration = calculateAcceleration(velocity, delta_time);
force = calculateForce(mass, acceleration);
std::cout << "calculated acceleration: " << acceleration << std::endl;
std::cout << "calculated force: " << force << std::endl;
}
return 0;
}
// ============================================================================
/// Clauclates aerodynamic forxe, in world coordinates [N]
QPointF ControlSurface::aerodynamicForce() const
{
QPointF result;
if ( body() && body()->b2body() )
{
b2Body* pBody = body()->b2body();
double bodyAngle = pBody->GetAngle(); // body angle
b2Vec2 velocity = pBody->GetLinearVelocity();
double v = velocity.Length();
double velAngle = atan2( velocity.y, velocity.x );
double angle = ( _value*_step + _inclination ) *parent()->orientation();
double attack = angle + bodyAngle - velAngle; // angle of attack
double sina = sin( attack );
QPointF force = calculateForce( v, sina ); // f0rce in wing coordinates
result = QPointF
( force.x()*cos(bodyAngle) + force.y()*sin(bodyAngle)
, force.x()*sin(bodyAngle) - force.y()*cos(bodyAngle)
);
}
return result;
}
void UCalculator::handleParticlePair(Particle& p1, Particle& p2) {
if(!(p1.getX().operator ==(p2.getX()))) {
double force = calculateForce(p1, p2);
p1.setU(p1.getU() + force);
}
//std::cout<<_P1.getU()<< std::endl;
//p2.setU(p2.getU() + force);
}
void CProjectile::init(float _x, float _y, int _parentID, bool _salve)
{
ci_projectile_data = new CProjectileData();
std::string sProjectileSound;
sProjectileSound = ci_projectile_data->getSound(_parentID);
if(!_salve)
{
sProjectileSound += ".wav";
PlaySound(glDataBox->GetSoundBuffer(sProjectileSound), _parentID + 4); // Make sure sounds get played on different layers
}
else
{
sProjectileSound += "_salve.wav";
PlaySound(glDataBox->GetSoundBuffer(sProjectileSound), _parentID + 4);
}
m_fStartX = _x;
m_fStartY = _y;
m_iParent = _parentID;
m_bSalve = _salve;
setBlocked(-1);
setScale(-1.0f, 1.0f);
setPosition(_x, _y);
setOrigin(ci_projectile_data->getOrigin(m_iParent).x, ci_projectile_data->getOrigin(m_iParent).y);
m_collision = new pwCollisionRect();
m_collision->SetSize((float)ci_projectile_data->getSize(m_iParent).width, (float)ci_projectile_data->getSize(m_iParent).height);
AddCollisionObject(ID_CO_PROJECTILE, m_collision);
SetGravityMultiplier(ci_projectile_data->getGravity(m_iParent) * (CBeatSystem::GetBPM() * 1.5f) / 80.0f); // Use BMP to modify gravity to a certain extent -> projectile needs to hit coloss
m_visual.SetFramesPerSecond(ci_projectile_data->getAnimationSpeed(m_iParent));
velocity temp = calculateForce(m_iParent);
SetStartVelocity(temp.xVel, temp.yVel);
m_visual.setTexture(*glDataBox->GetTexture(ci_projectile_data->getTexture(m_iParent)));
m_visual.SetFrameSize(ci_projectile_data->getSize(m_iParent).width, ci_projectile_data->getSize(m_iParent).height);
m_visual.SetTotalFrames(ci_projectile_data->getFrames(m_iParent));
m_visual.AddAnimation(P_ANIM_NORMAL, ci_projectile_data->getAnimation(m_iParent).normalStart, ci_projectile_data->getAnimation(m_iParent).normalEnd);
m_visual.AddAnimation(P_ANIM_DEATH, ci_projectile_data->getAnimation(m_iParent).deathStart, ci_projectile_data->getAnimation(m_iParent).deathEnd);
m_visual.SetCurrentAnimation(P_ANIM_NORMAL);
AddDrawableObject(ID_DO_PROJECTILE, &m_visual);
m_vProjectiles.push_back(this);
glLogics->RegisterGameObject(this, ID_GS_PLAY);
}
void ODE_LinearSpringModel::updateInputValues() {
if(mOwnerSpringAdapter == 0 || mOwnerSpringAdapter->getCurrentTargetJoint() == 0) {
return;
}
//The first time an update is run the mJoint is located.
//This has to be done here instead of setup() to ensure that the joint is
//really available, which can not be guaranteed in setup().
if(mJoint == 0) {
SimJoint *simJoint = mOwnerSpringAdapter->getCurrentTargetJoint();
ODE_Joint *odeJoint = dynamic_cast<ODE_Joint*>(simJoint);
if(odeJoint == 0) {
MotorAdapter *adapter = dynamic_cast<MotorAdapter*>(simJoint);
if(adapter != 0) {
odeJoint = dynamic_cast<ODE_Joint*>(adapter->getActiveMotorModel());
}
}
if(simJoint != 0 && odeJoint != 0) {
if(getType() == MotorModel::HINGE_JOINT) {
if(dynamic_cast<HingeJoint*>(simJoint) != 0) {
mJoint = odeJoint->getJoint();
}
}
else if(getType() == MotorModel::SLIDER_JOINT) {
if(dynamic_cast<SliderJoint*>(simJoint) != 0) {
mJoint = odeJoint->getJoint();
}
}
}
}
if(mJoint != 0) {
if(getType() == MotorModel::HINGE_JOINT) {
double torque = calculateTorque(dJointGetHingeAngle(mJoint));
dJointAddHingeTorque(mJoint, torque);
mOwnerSpringAdapter->getCurrentTorqueValue()->set(torque);
}
else if(getType() == MotorModel::SLIDER_JOINT) {
double force = calculateForce(dJointGetSliderPosition(mJoint));
dJointAddSliderForce(mJoint, force);
mOwnerSpringAdapter->getCurrentTorqueValue()->set(force);
}
}
}
void LJForceCalculator::handleParticlePair(Particle& p1, Particle& p2) {
utils::Vector<double,3> force = calculateForce(p1, p2);
p1.getF() = p1.getF() + force;
if(_setP2) p2.getF() = p2.getF() - force;
}
void Robot::calculatePosition(int32_t xFieldSize, int32_t yFieldSize, boost::shared_ptr<Robot> secondRobot, int timeDelay)
{
// tu odpalamy calculateForce i na podstawie wyliczonych wartości
// siły oraz bieżących prędkości wyliczamy nowe położenie
const double timeStep = static_cast<double>(timeDelay)/1000; //[s]
const double robotMass = 1; //[kg]
const double maxVelocityX = 0.2*xFieldSize;
const double maxVelocityY = 0.2*yFieldSize;
const double frictionFactor = 0;
const double brakingFactor = 0.3;
bool brake = false;
Force force = calculateForce(xFieldSize, yFieldSize, secondRobot);
/* Siła tłumiąca */
if(force.X > abs(frictionFactor*robotMass*9.81))
force.X += frictionFactor*(-sgn(_xVel))*robotMass*9.81;
else
brake = true;
if(force.Y > abs(frictionFactor*robotMass*9.81))
force.Y += frictionFactor*(-sgn(_yVel))*robotMass*9.81;
else
brake = true;
/* Ograniczenie przemieszczenia zgodnie z maxVelocity */
if((timeStep*_xVel + (force.X/robotMass)*pow(timeStep,2)/2) > maxVelocityX*timeStep)
{
_xPos = getXPos() + timeStep*maxVelocityX;
}
else if((timeStep*_xVel + (force.X/robotMass)*pow(timeStep,2)/2) < -maxVelocityX*timeStep)
{
_xPos = getXPos() - timeStep*maxVelocityX;
}
else
{
_xPos = getXPos() + timeStep*_xVel + (force.X/robotMass)*pow(timeStep,2)/2;
}
if(timeStep*_yVel + (force.Y/robotMass)*pow(timeStep,2)/2 > maxVelocityY*timeStep)
{
_yPos = getYPos() + timeStep*maxVelocityY;
}
else if(timeStep*_yVel + (force.Y/robotMass)*pow(timeStep,2)/2 < -maxVelocityY*timeStep)
{
_yPos = getYPos() - timeStep*maxVelocityY;
}
else
{
_yPos = getYPos() + timeStep*_yVel + (force.Y/robotMass)*pow(timeStep,2)/2;
}
_xVel += (force.X/robotMass)*timeStep;
if(brake) _xVel *= brakingFactor;
_yVel += (force.Y/robotMass)*timeStep;
if(brake) _yVel *= brakingFactor;
if(_xVel > maxVelocityX) _xVel = maxVelocityX;
else if(_xVel < -maxVelocityX) _xVel = -maxVelocityX;
if(_yVel > maxVelocityY) _yVel = maxVelocityY;
else if(_yVel < -maxVelocityY) _yVel = -maxVelocityY;
}
int main(int argc, char *argv[])
{
int i, N=2500;
const float L=1,W=1,dt=1e-6;
G=2.0/N;
double *x,*y,*mass,*forceX,*forceY,*u,*v,ax,ay;
const int frameskip=100;
int frame=frameskip-1;
const double initSpeedLimit=0.001;
const float theta = 0.8;
x = (double *)malloc(N*sizeof(double));
y = (double *)malloc(N*sizeof(double));
u = (double *)malloc(N*sizeof(double));
v = (double *)malloc(N*sizeof(double));
mass = (double *)malloc(N*sizeof(double));
forceX = (double *)malloc(N*sizeof(double));
forceY = (double *)malloc(N*sizeof(double));
#ifndef DISTRIBUTION_AS_IN_ASSIGNMENT
for(i=0;i<N;i++)
{
x[i]=frand(0,1);
y[i]=frand(.25,.75);
relativePosition(&u[i], &v[i], 0, 0, x[i], y[i]);
u[i]*=frand(-initSpeedLimit,initSpeedLimit);
v[i]*=frand(-initSpeedLimit,initSpeedLimit);
mass[i]=frand(1, 100);
forceX[i]=0;
forceY[i]=0;
}
x[0]=x[1]=0.5;
y[0]=0.45; y[1]=0.55;
v[0]=v[1]=0;
u[0]=-0.007;
u[1]=0.007;
mass[0]=mass[1]=1000;
#endif
#ifdef DISTRIBUTION_AS_IN_ASSIGNMENT
const double alpha=0.25;
const double V=20.0;
for(i = 0; i<N; i++)
{
mass[i]=1;
double R=frand(0, L/4.0);
double fi=frand(0, 2*3.1415);
x[i]=L/2.0+R*cos(fi);
y[i]=W/2.0+alpha*R*sin(fi);
double Rprim = sqrt((x[i]-L/2.0)*(x[i]-L/2.0) + (y[i]-W/2.0)*(y[i]-W/2.0));
u[i] = -V*Rprim*sin(fi);
v[i] = V*Rprim*cos(fi);
forceX[i]=0;
forceY[i]=0;
}
#endif
InitializeGraphics(argv[0],windowWidth,windowWidth);
SetCAxes(0,1);
Node *root;
printf("Hit q to quit.\n");
int iterations=0;
while(!CheckForQuit())
{
iterations++;
#ifndef BARNESHUT
for(i=0;i<N;i++) {
#endif
#ifdef BARNESHUT
// Bounce(&x[0],&y[0],&u[0],&v[0],L,W);
root = createNode(0,x,y,mass);
for(i=1;i<N;i++) {
#endif
if (x[i] > 2*L || x[i]< -1 || y[i] > 2*W || y[i] < -1)
{
printf("Things are going out of bounds");
exit(-1);
}
// Bounce(&x[i],&y[i],&u[i],&v[i],L,W);
#ifndef BARNESHUT
calculateForce(&forceX[i], &forceY[i], i, x, y, mass, N);
#endif
#ifdef BARNESHUT
insertParticle(root, x, y, mass, i);
#endif
}
#ifdef BARNESHUT
for(i=0; i<N; i++)
BarnesHut(root, x, y, mass, theta, i, &forceX[i], &forceY[i]);
#endif
update(x,y,u,v,mass,forceX, forceY,dt, N);
frame++;
frame=frame%frameskip;
if(frame==0) {
ClearScreen();
for(i=0;i<N;i++)
DrawCircle(x[i],y[i],L,W,(1+log10(mass[i]))*circleRadius,circleColor);
Refresh();
}
freeNodes(root);
}
XFlush(display);
XCloseDisplay(display);
printf("Iterations: %d\n", iterations);
return 0;
}
/*
* Function: Bounce
* Usage: Bounce(&x[i],&y[i],&u[i],&v[i],L,W);
* -------------------------------------------
* If a particle moves beyond any of the boundaries then set
* it on the other side of the boundary back in the domain and
* reverse the velocities.
*
*/
void Bounce(double *x, double *y, double *u, double *v, double L, double W) {
if(*x>L ) {
*x=2*L-*x;
*u=-*u;
}
if(*x<0 ) {
*x=-*x;
*u=-*u;
}
if(*y>W ) {
*y=2*W-*y;
*v=-*v;
}
if(*y<0 ) {
*y=-*y;
*v=-*v;
}
}
