//struct sphere wall_check( struct sphere ball ) {
void wall_check( struct sphere *ball ) {
//return ball;
double dist = 5.0 - ball->radius;
double dist_top = 10.0 - ball->radius;
double dist_bottom = 0.0 + ball->radius;
if(ball->path == 1) {
ball->direction.x = ball->pos.x - ball->previous_pos.x;
ball->direction.y = ball->pos.y - ball->previous_pos.y;
// 3D
ball->direction.z = ball->pos.z - ball->previous_pos.z;
normalize_dir(ball);
}
if( ball->pos.x >= dist || ball->pos.x <= -1*dist) {
ball->direction.x *= -1;
ball->pos.x = ( ball->pos.x < 0.0 ) ? -1*dist : dist;
ball->path = 0;
ball->start_time = (double) clock();
ball->active = 1;
} else if(ball->pos.y >= dist_top || ball->pos.y <= dist_bottom) {
ball->direction.y *= -1;
ball->pos.y = ( ball->pos.y < 5.0 ) ? dist_bottom : dist_top;
ball->path = 0;
ball->start_time = (double) clock();
ball->active = 1;
// 3D
} else if(ball->pos.z >= dist || ball->pos.z <= -1*dist) {
ball->direction.z *= -1;
ball->pos.z = ( ball->pos.z < 0.0 ) ? -1*dist : dist;
ball->path = 0;
ball->start_time = (double) clock();
ball->active = 1;
}
}
char *get_new_chain_vrml_path(const char *dir, int n)
{
static char file_path[MAX_PATH_LENGTH];
strcpy(file_path, dir);
normalize_dir(file_path);
sprintf(file_path, "%s/newchain%d.wrl", file_path, n);
return file_path;
}
char* full_path(const char *dir, const char *file)
{
static char file_path[MAX_PATH_LENGTH];
strcpy(file_path, dir);
normalize_dir(file_path);
strcat(file_path, "/");
strcat(file_path, file);
return file_path;
}
void DetectWithOF5::draw_motions_dense_of()
{
cv::Mat vis = cv::Mat::zeros(cv::Size(origin_.cols, origin_.rows), CV_32FC3); // rgb 格式为 32FC3
for (size_t i = 0; i < motions_.size(); i++) {
Motion &m = motions_[i];
cv::Mat rgb;
if (m.dense_sum_dis.cols > 0) {
cv::Mat ndis, ndir;
normalize_dis(m.dense_sum_dis, ndis);
normalize_dir(m.dense_sum_dir, ndir);
rgb_from_dis_ang(ndis, ndir, rgb);
cv::Mat r = vis(m.brc);
rgb.copyTo(r);
}
}
//cv::imshow("dense of", vis);
}
//struct sphere wall_check( struct sphere ball ) {
void wall_check( struct sphere *ball ) {
//return ball;
double dist = 5.0 - ball->radius;
if(ball->path == 1) {
ball->direction.x = ball->pos.x - ball->previous_pos.x;
ball->direction.y = ball->pos.y - ball->previous_pos.y;
normalize_dir(ball);
}
if( ball->pos.x >= dist || ball->pos.x <= -1*dist) {
ball->direction.x *= -1;
ball->pos.x = ( ball->pos.x < 0.0 ) ? -1*dist : dist;
ball->path = 0;
ball->start_time = (double) clock();
ball->active = 1;
} else if(ball->pos.y >= dist || ball->pos.y <= -1*dist) {
ball->direction.y *= -1;
ball->pos.y = ( ball->pos.y < 0.0 ) ? -1*dist : dist;
ball->path = 0;
ball->start_time = (double) clock();
ball->active = 1;
}
}
/*
* void collision_response( struct sphere *b1, struct sphere *b2);
*
* performs collision response between two balls
*/
void collision_response(struct sphere *b1, struct sphere *b2) {
b1->active = 1;
b2->active = 1;
double dx = b1->pos.x - b2->pos.x;
double dy = b1->pos.y - b2->pos.y;
double collision_angle = atan2(dy, dx);
double b1_v, b2_v;
b1_v = b1->velocity;
b2_v = b2->velocity;
double b1_vx, b1_vy, b2_vx, b2_vy;
b1_vx = (b1->direction.x * b1_v);
b1_vy = (b1->direction.y * b1_v);
b2_vx = (b2->direction.x * b2_v);
b2_vy = (b2->direction.y * b2_v);
double mag1 = sqrt( b1_vx*b1_vx + b1_vy*b1_vy);
double mag2 = sqrt( b2_vx*b2_vx + b2_vy*b2_vy);
double dir1 = atan2(b1_vy, b1_vx);
double dir2 = atan2(b2_vy, b2_vx);
double b1_vx_new, b1_vy_new;
double b2_vx_new, b2_vy_new;
b1_vx_new = mag1 * cos(dir1-collision_angle);
b1_vy_new = mag1 * sin(dir1-collision_angle);
b2_vx_new = mag2 * cos(dir2-collision_angle);
b2_vy_new = mag2 * sin(dir2-collision_angle);
double b1_vx_final, b1_vy_final;
double b2_vx_final, b2_vy_final;
// get ball masses
double m1 = get_mass(*b1), m2 = get_mass(*b2);
b1_vx_final = (( m1 - m2)*b1_vx_new +
(m2+m2)*b2_vx_new)/(m1+m2);
b2_vx_final = (( m1 + m1)*b1_vx_new +
(m2-m1)*b2_vx_new)/(m1+m2);
b1_vy_final = b1_vy_new;
b2_vy_final = b2_vy_new;
b1_vx = cos(collision_angle)*b1_vx_final +
cos(collision_angle+PI/2)*b1_vy_final;
b1_vy = sin(collision_angle)*b1_vx_final +
sin(collision_angle+PI/2)*b1_vy_final;
b2_vx = cos(collision_angle)*b2_vx_final +
cos(collision_angle+PI/2)*b2_vy_final;
b2_vy = sin(collision_angle)*b2_vx_final +
sin(collision_angle+PI/2)*b2_vy_final;
b1->velocity =
sqrt( pow(b1_vx,2) + pow(b1_vy,2));
b2->velocity =
sqrt( pow(b2_vx,2) + pow(b2_vy,2));
b1->direction.x = b1_vx;
b1->direction.y = b1_vy;
normalize_dir(b1);
b2->direction.x = b2_vx;
b2->direction.y = b2_vy;
normalize_dir(b2);
b1->path = 0;
b2->path = 0;
b1->start_time = (double) clock();
b2->start_time = (double) clock();
//b1->active = 1;
//b2->active = 1;
// store before collision velocities
//double b1_v, b2_v;
//b1_v = b1->velocity;
//b2_v = b2->velocity;
//double b1_vx, b1_vy, b2_vx, b2_vy;
//b1_vx = (b1->direction.x * b1_v);
//b1_vy = (b1->direction.y * b1_v);
//b2_vx = (b2->direction.x * b2_v);
//b2_vy = (b2->direction.y * b2_v);
// have x and y components of speed
// get ball masses
//double m1 = get_mass(*b1), m2 = get_mass(*b2);
// need the new velocity components (after collision)
//double b1_vx_new, b1_vy_new, b2_vx_new, b2_vy_new;
// ball 1 new components
//b1_vx_new = ((m1-m2) * b1_vx + (2*m2) * b2_vx)/(m1+m2);
//b1_vy_new = ((m1-m2) * b1_vy + (2*m2) * b2_vy)/(m1+m2);
// ball 2 new components
//b2_vx_new = ((m2-m1) * b2_vx + (2*m1) * b1_vx)/(m1+m2);
//b2_vy_new = ((m2-m1) * b2_vy + (2*m1) * b1_vy)/(m1+m2);
// need to change direction to match new speeds
//b1->direction.x = b1_vx_new;
//b1->direction.y = b1_vy_new;
//normalize_dir(b1);
//b2->direction.x = b2_vx_new;
//b2->direction.y = b2_vy_new;
//normalize_dir(b2);
//b1->velocity =
// sqrt( pow(b1_vx_new,2) + pow(b1_vy_new,2));
//b2->velocity =
// sqrt( pow(b2_vx_new,2) + pow(b2_vy_new,2));
// speeds updated
//b1->path = 0;
//b2->path = 0;
//b1->start_time = (double) clock();
//b2->start_time = (double) clock();
}
/*
* void update_direction( struct sphere *ball);
*
* Generates a direction based on the previous direction. Used when a sphere
* travelling on a curve path, become a straight path.
*/
void update_direction( struct sphere *ball ) {
ball->direction.x = ball->pos.x - ball->previous_pos.x;
ball->direction.y = ball->pos.y - ball->previous_pos.y;
normalize_dir(ball);
}
/*
* Generates a direction based on the previous direction. Used when a sphere
* travelling on a curve path, become a straight path.
*/
void Sphere::update_direction() {
direction.x = pos.x - previous_pos.x;
direction.y = pos.y - previous_pos.y;
direction.z = pos.z - previous_pos.z;
normalize_dir();
}
/*
* void collision_response( struct sphere *b1, struct sphere *b2);
*
* performs collision response between two balls
*/
void collision_response(struct sphere *b1, struct sphere *b2) {
b1->active = 1;
b2->active = 1;
if(response == 1) {
// proper 3D collisions using equation provided on
// http://www.plasmaphysics.org.uk/programs/coll3d_cpp.htm
double R = 1.0;
double m1 = get_mass(b1->radius);
double m2 = get_mass(b2->radius);
double r1 = b1->radius;
double r2 = b2->radius;
double x1 = b1->pos.x;
double y1 = b1->pos.y;
double z1 = b1->pos.z;
double x2 = b2->pos.x;
double y2 = b2->pos.y;
double z2 = b2->pos.z;
double vx1 = (b1->direction.x * b1->velocity);
double vy1 = (b1->direction.y * b1->velocity);
double vz1 = (b1->direction.z * b1->velocity);
double vx2 = (b2->direction.x * b2->velocity);
double vy2 = (b2->direction.y * b2->velocity);
double vz2 = (b2->direction.z * b2->velocity);
double pi,r12,m21,d,v,theta2,phi2,st,ct,sp,cp,vx1r,vy1r,vz1r,fvz1r,
thetav,phiv,dr,alpha,beta,sbeta,cbeta,dc,sqs,t,a,dvz2,
vx2r,vy2r,vz2r,x21,y21,z21,vx21,vy21,vz21,vx_cm,vy_cm,vz_cm;
// **** initialize some variables ****
pi=acos(-1.0E0);
//error=0;
r12=r1+r2;
m21=m2/m1;
x21=x2-x1;
y21=y2-y1;
z21=z2-z1;
vx21=vx2-vx1;
vy21=vy2-vy1;
vz21=vz2-vz1;
vx_cm = (m1*vx1+m2*vx2)/(m1+m2) ;
vy_cm = (m1*vy1+m2*vy2)/(m1+m2) ;
vz_cm = (m1*vz1+m2*vz2)/(m1+m2) ;
// **** calculate relative distance and relative speed ***
d=sqrt(x21*x21 +y21*y21 +z21*z21);
v=sqrt(vx21*vx21 +vy21*vy21 +vz21*vz21);
// **** shift coordinate system so that ball 1 is at the origin ***
x2=x21;
y2=y21;
z2=z21;
// **** boost coordinate system so that ball 2 is resting ***
vx1=-vx21;
vy1=-vy21;
vz1=-vz21;
// **** find the polar coordinates of the location of ball 2 ***
theta2=acos(z2/d);
if (x2==0 && y2==0) {phi2=0;} else {phi2=atan2(y2,x2);}
st=sin(theta2);
ct=cos(theta2);
sp=sin(phi2);
cp=cos(phi2);
// **** express the velocity vector of ball 1 in a rotated coordinate
// system where ball 2 lies on the z-axis ******
vx1r=ct*cp*vx1+ct*sp*vy1-st*vz1;
vy1r=cp*vy1-sp*vx1;
vz1r=st*cp*vx1+st*sp*vy1+ct*vz1;
fvz1r = vz1r/v ;
if (fvz1r>1) {fvz1r=1;} // fix for possible rounding errors
else if (fvz1r<-1) {fvz1r=-1;}
thetav=acos(fvz1r);
if (vx1r==0 && vy1r==0) {phiv=0;} else {phiv=atan2(vy1r,vx1r);}
// **** calculate the normalized impact parameter ***
dr=d*sin(thetav)/r12;
// **** calculate impact angles if balls do collide ***
alpha=asin(-dr);
beta=phiv;
sbeta=sin(beta);
cbeta=cos(beta);
// **** calculate time to collision ***
t=(d*cos(thetav) -r12*sqrt(1-dr*dr))/v;
// **** update positions and reverse the coordinate shift ***
x2=x2+vx2*t +x1;
y2=y2+vy2*t +y1;
z2=z2+vz2*t +z1;
x1=(vx1+vx2)*t +x1;
y1=(vy1+vy2)*t +y1;
z1=(vz1+vz2)*t +z1;
// *** update velocities ***
a=tan(thetav+alpha);
dvz2=2*(vz1r+a*(cbeta*vx1r+sbeta*vy1r))/((1+a*a)*(1+m21));
vz2r=dvz2;
vx2r=a*cbeta*dvz2;
vy2r=a*sbeta*dvz2;
vz1r=vz1r-m21*vz2r;
vx1r=vx1r-m21*vx2r;
vy1r=vy1r-m21*vy2r;
// **** rotate the velocity vectors back and add the initial velocity
// vector of ball 2 to retrieve the original coordinate system ****
vx1=ct*cp*vx1r-sp*vy1r+st*cp*vz1r +vx2;
vy1=ct*sp*vx1r+cp*vy1r+st*sp*vz1r +vy2;
vz1=ct*vz1r-st*vx1r +vz2;
vx2=ct*cp*vx2r-sp*vy2r+st*cp*vz2r +vx2;
vy2=ct*sp*vx2r+cp*vy2r+st*sp*vz2r +vy2;
vz2=ct*vz2r-st*vx2r +vz2;
b1->direction.x = vx1;
b1->direction.y = vy1;
// 3D
b1->direction.z = vz1;
normalize_dir(b1);
b2->direction.x = vx2;
b2->direction.y = vy2;
b2->direction.z = vz2;
normalize_dir(b2);
b1->velocity =
sqrt( pow(vx1,2) + pow(vy1,2) + pow(vz1,2));
b2->velocity =
sqrt( pow(vx2,2) + pow(vy2,2) + pow(vz2,2));
}else{
// store before collision velocities
double b1_v, b2_v;
b1_v = b1->velocity;
b2_v = b2->velocity;
double b1_vx, b1_vy, b2_vx, b2_vy;
// 3D
double b1_vz, b2_vz;
b1_vx = (b1->direction.x * b1_v);
b1_vy = (b1->direction.y * b1_v);
// 3D
b1_vz = (b1->direction.z * b1_v);
b2_vx = (b2->direction.x * b2_v);
b2_vy = (b2->direction.y * b2_v);
// 3D
b2_vz = (b2->direction.z * b2_v);
// have x and y components of speed
// get ball masses
double m1 = get_mass(b1->radius), m2 = get_mass(b2->radius);
// need the new velocity components (after collision)
double b1_vx_new, b1_vy_new, b2_vx_new, b2_vy_new;
// 3D
double b1_vz_new, b2_vz_new;
// ball 1 new components
b1_vx_new = ((m1-m2) * b1_vx + (2*m2) * b2_vx)/(m1+m2);
b1_vy_new = ((m1-m2) * b1_vy + (2*m2) * b2_vy)/(m1+m2);
// 3D
b1_vz_new = ((m1-m2) * b1_vz + (2*m2) * b2_vz)/(m1+m2);
// ball 2 new components
b2_vx_new = ((m2-m1) * b2_vx + (2*m1) * b1_vx)/(m1+m2);
b2_vy_new = ((m2-m1) * b2_vy + (2*m1) * b1_vy)/(m1+m2);
// 3D
b2_vz_new = ((m2-m1) * b2_vz + (2*m1) * b1_vz)/(m1+m2);
// need to change direction to match new speeds
b1->direction.x = b1_vx_new;
b1->direction.y = b1_vy_new;
// 3D
b1->direction.z = b1_vz_new;
normalize_dir(b1);
b2->direction.x = b2_vx_new;
b2->direction.y = b2_vy_new;
b2->direction.z = b2_vz_new;
normalize_dir(b2);
b1->velocity =
sqrt( pow(b1_vx_new,2) + pow(b1_vy_new,2) + pow(b1_vz_new,2));
b2->velocity =
sqrt( pow(b2_vx_new,2) + pow(b2_vy_new,2) + pow(b2_vz_new,2));
// speeds updated
}
b1->path = 0;
b2->path = 0;
b1->start_time = (double) clock();
b2->start_time = (double) clock();
}
