正の足し算のはずが負になる。
Posted: 2013年7月30日(火) 01:05
現在粒子の相互作用を表すプログラムをつくってるいるんですが密度はq+=m*w[j]で二重ループで計算しているんですが密度が何故か負になってしまいます。 w[j]は正なので負なるはずはないんですが。。。
#include <stdlib.h>
#include<stdio.h>
#include <GL/glut.h>
#include <math.h>
const float h=20;
#define N 3
float g= 9.8;
const float K=1000;
const float q0=1;
const float n=1;
int width = 600, height = 600;
/* 初期値 */
float y[N] = {10.,10,10};
float Vy[N];
float t =0;
float dt =0.01;
float tmax=10.;
float x[N] = {-10,0,10};
float Vx[N];
const float pi=3.1415;
const float a1=4/(pi*powf(h,8.));
const float a2=-30/(pi*powf(h,5));
const float m=1;
/*距離の計算*/
inline void distance(float s[],float k[], float dis[][N],float dis_2[][N]){
int i,j;
for(i=0; i<N; i++){
for(j=0; j<N;j++){
dis[i][j]=sqrtf( powf(s[j]-s[i],2)+powf(k[j]-k[i],2));
dis_2[i][j]= powf(s[j]-s[i],2)+powf(k[j]-k[i],2);
}
}
}
/*位置ベクトルの差*/
inline void residual_vector(float s[], float k[], float res_x[][N],float res_y[][N]){
int i,j;
for(i=0; i<N; i++){
for(j=0; j<N; j++){
res_x[i][j]=s[j]-s[i];
res_y[i][j]=k[j]-k[i];
}
}
}
/*重み関数*/
inline void weight(float s[] ,float k[], float w[][N] ){
int i,j;
float dis[N][N], dis_2[N][N];
distance(s,k,dis ,dis_2);
for(i=0; i<N; i++){
for(j=0; j<N; j++){
w[i][j]=a1*powf((h*h-dis_2[i][j]),3);
}
}
}
/*勾配*/
inline void grad(float s[N], float k[N], float g_x[][N] ,float g_y[][N]){
int i,j;
float dis[N][N],res_x[N][N],res_y[N][N],dis_2[N][N];
distance(x,y,dis,dis_2);
residual_vector( s, k, res_x, res_y);
for(i=0; i<N; i++){
for(j=0; j<N; j++){
g_x[i][j]=a2*powf((h-dis[i][j]),2)*res_x[i][j]/dis[i][j];
g_y[i][j]=a2*powf((h-dis[i][j]),2)*res_y[i][j]/dis[i][j];
}
}
}
/*発散*/
void Laplacian(float s[N], float k[N], float L[][N]){
int i,j;
float dis[N][N],res_x[N][N],res_y[N][N],dis_2[N][N];
residual_vector( s, k, res_x, res_y);
for(i=0; i<N; i++){
for(j=0; j<N; j++){
if(dis[i][j] >= 0.0 && dis[i][j] < h){
L[i][j]=a2*(3*powf((h*h-dis_2[i][j]),2)-4*h*h-dis_2[i][j]);
}
else{
L[i][j]=0;
}
}
}
}
/*密度*/
inline void density(float x[] ,float y[] ,float q[]){
float w[N][N];
weight(x,y,w);
int i,j;
for(i=0; i<N; i++){
for(j=0; j<N; j++){
q[i]+=m*w[i][j];
}
}
}
/*圧力*/
inline void presure(float s[],float k[],float p[]){
int i;
float q[N];
density(s,k,q);
for(i=0; i<N; i++){
p[i]=K*(q[i]-q0);
}
}
/*運動方程式*/
float *Fx(float s[] , float k[],float Vs[],float Vk[]){
float dis[N][N],res_x[N][N],res_y[N][N],dis_2[N][N],g_x[N][N],g_y[N][N],q[N],p[N],w[N][N];
static float Fx[N];
distance(s,k,dis,dis_2);
weight(s,k,w);
residual_vector(s,k,res_x,res_y);
grad(s,k,g_x,g_y);
density(s,k,q);
presure(s,k,p);
int j,i;
for(i=0; i<N; i++){
for(j=0; j<N; j++){
if (i==j )
Fx[i]+=0;
else if(dis[i][j]<h)
Fx[i]+=-m*((p[i]+p[j])/(2*q[i]))*g_x[i][j];
else
Fx[i]+=0;
}
}
return Fx ;
}
float * Fy(float s[] , float k[],float Vs[],float Vk[]){
float dis[N][N],res_x[N][N],res_y[N][N],dis_2[N][N],p[N],g_x[N][N],g_y[N][N],q[N];
static float Fy[N];
int j,i;
for(i=0; i<N; i++){
for(j=0; j<N; j++){
if (i==j )
Fy[i]+=-g;
else if(dis[i][j]<h)
Fy[i]+=0;
else
Fy[i]+=0;
}
}
return Fy ;
}
void Calculate(float s[],float k[],float Vs[],float Vk[])
{
float kx2[N], kx1[N], ky1[N],ky2[N],kx3[N],ky3[N],kx4[N],ky4[N];
int i;
for(i=0;i<N;i++) {
kx1[i]=dt**(Fx(s,k,Vs,Vk)+i);
ky1[i]=dt**(Fy(s,k,Vs,Vk)+i);
s[i]=s[i]+kx1[i]/2.0;
k[i]=k[i]+ky1[i]/2.0;
kx2[i]=dt**(Fx(s,k,Vs,Vk)+i);
ky2[i]=dt**(Fy(s,k,Vs,Vk)+i);
s[i]=s[i]+kx2[i]/2.0-kx1[i]/2.0;
k[i]=k[i]+ky2[i]/2.0-ky1[i]/2.0;
kx3[i]=dt**(Fx(s,k,Vs,Vk)+i);
ky3[i]=dt**(Fy(s,k,Vs,Vk)+i);
s[i]=s[i]+kx3[i]/2.0-kx2[i]/2.0;
k[i]=k[i]+ky3[i]/2.0-ky2[i]/2.0;
kx4[i]=dt**(Fx(s,k,Vs,Vk)+i);
ky4[i]=dt**(Fy(s,k,Vs,Vk)+i);
s[i]=s[i]-kx3[i]/2.0;
k[i]=k[i]-ky3[i]/2.0;
Vx[i]=Vx[i]+(kx1[i]+2.0*kx2[i]+2.0*kx3[i]+kx4[i])/6.0;
Vy[i]=Vy[i]+(ky1[i]+2.0*ky2[i]+2.0*ky3[i]+ky4[i])/6.0;
s[i] += Vs[i]*dt;
k[i] += Vk[i]*dt;
*(Fx(s,k,Vs,Vk)+i)=0;
*(Fy(s,k,Vs,Vk)+i)=0;
}
}
void particle(void){
int i;
/* axis */
glColor3f(.2, .3, .5);
glEnd();
/* draw points */
glColor3f( .9, .2, .2 );
glPointSize(5.);
glBegin( GL_POINTS );
for(i=0;i<N;i++)
glVertex2f( x[i], y[i]);
glEnd();
}
void display(void){
glClearColor( .0, .0, .0, .0);
glClear( GL_COLOR_BUFFER_BIT );
particle();
glutSwapBuffers();
}
void idle(void){
int i;
float g_x[N][N],g_y[N][N],dis[N][N],dis_2[N][N],q[N],w[N][N];
grad(x,y,g_x,g_y);
distance( x, y, dis, dis_2);
Calculate(x,y,Vx,Vy);
density(x,y,q);
weight(x,y,w);
printf("%e %f %e %f %f\n",a2,dis_2[0][1],g_x[0][1],q[1],w[1][1]);
for(i=0; i<N; i++){
/* 衝突 */
if(x[i]>100.){
x[i]=100.;
Vx[i]=-0.9*Vx[i];
}
if(y[i]<-100.){
y[i]=-100.;
Vy[i]=-0.9*Vy[i];
}
if(x[i]<-100.){
x[i]=-100.;
Vx[i]=-0.9*Vx[i];
}
if(y[i]>100.){
y[i]=100;
Vy[i]=-0.9*Vy[i];
}
glutPostRedisplay();
}
}
int main(int argc, char** argv){
/* init gult */
glutInit( &argc, argv );
glutInitDisplayMode( GLUT_DOUBLE | GLUT_RGB );
glutInitWindowPosition( 0, 0 );
glutInitWindowSize( width, height );
glutCreateWindow( "particle" );
/* call-back */
glutDisplayFunc( display );
glutIdleFunc( idle );
/* 2D-setup */
glMatrixMode( GL_PROJECTION );
glLoadIdentity();
gluOrtho2D( -100., 100., -100., 100.);
glutMainLoop();
return 0;
}