void init_physics(simulation_t* sim)
{
int i;
calculate_forces(sim);
for(i=0;i<sim->num_celestial_bodies;i++)sim->celestial_bodies[i].base.acceleration=vector_multiply(1/sim->celestial_bodies[i].base.mass,sim->celestial_bodies[i].base.force);
for(i=0;i<sim->num_spacecraft;i++)sim->spacecraft[i].base.acceleration=vector_multiply(1/sim->spacecraft[i].base.mass,sim->spacecraft[i].base.force);
}
void simulation_render_lagrange_points(simulation_t* sim,celestial_body_t* secondary)
{
vector_t lagrange_points[5];
vector_t rel_position=vector_subtract(secondary->base.position,secondary->orbit.primary->base.position);
vector_t direction=vector_normalize(rel_position);
float a=secondary->base.mass/(secondary->base.mass+secondary->orbit.primary->base.mass);
float offset=powf(a*0.3333333,0.333333);
lagrange_points[0]=vector_multiply(secondary->orbit.semi_major_axis*(1+offset),direction);
lagrange_points[1]=vector_multiply(secondary->orbit.semi_major_axis*(1-offset),direction);
lagrange_points[2]=vector_multiply(-secondary->orbit.semi_major_axis*(1+(5.0/12.0)*a),direction);
lagrange_points[3].x=rel_position.x*cos(M_PI/3)+rel_position.y*sin(M_PI/3);
lagrange_points[3].y=rel_position.x*-sin(M_PI/3)+rel_position.y*cos(M_PI/3);
lagrange_points[4].x=rel_position.x*cos(-M_PI/3)+rel_position.y*sin(-M_PI/3);
lagrange_points[4].y=rel_position.x*-sin(-M_PI/3)+rel_position.y*cos(-M_PI/3);
int i;
for(i=0;i<5;i++)
{
char str[128];
sprintf(str,"%s-%s L%d point",secondary->orbit.primary->name,secondary->name,i+1);
vector_t screen_pos=vector_transform(vector_add(secondary->orbit.primary->base.position,lagrange_points[i]),sim->camera);
draw_cross(screen_pos,get_color(0,0,255));
draw_text(screen_pos.x+10,screen_pos.y,str);
}
}
vector_t calculate_gravitation(object_t* a,object_t* b)
{
//Get distance and direction
vector_t displacement=vector_subtract(a->position,b->position);
double distance=vector_magnitude(displacement);
vector_t direction=vector_multiply(1/distance,displacement);
//Calculate gravity
double gravity=GRAVITATIONAL_CONSTANT*(a->mass*b->mass)/(distance*distance);
return vector_multiply(gravity,direction);
}
t_ray transform_ray(t_ray *ray, t_mat44 mat)
{
t_ray tray;
tray.origin = vector_multiply(ray->origin, mat);
tray.dir = normalize_vec3f(vector_dir_multiply(ray->dir, mat));
return (tray);
}
void object_step_velocity(object_t* object,double delta_t)
{
//Calculate A(t+dt)
object->acceleration=vector_multiply(1/object->mass,object->force);
//Calculate V(t+dt);
object->velocity.x+=0.5*object->acceleration.x*delta_t;
object->velocity.y+=0.5*object->acceleration.y*delta_t;
//Update rotation;
object->rotation+=object->delta_rot*delta_t;
}
double *projection(Matrix *m, double *v, int length){
unsigned int i, j;
double *sum, *copy, *vector, factor;
if(m->rows != length)
return NULL;
if(m == NULL || v == NULL)
return NULL;
sum = calloc(sizeof(double), m->rows);
copy = malloc(sizeof(double)*m->rows);
for(i = 0; i < m->columns; i++){
for(j = 0; j < m->rows; j++)
copy[j] = m->numbers[i][j];
vector = copy;
factor = vector_multiply(v, vector, m->rows)/vector_multiply(vector, vector, m->rows);
scalar_vector_multiplication(factor, vector, m->rows);
vector_addition(sum, vector, m->rows);
}
free(copy);
return sum;
}
// equation (3) from the paper
HRESULT ParticleSwarmOptimizer::update_velocity( Particle& p ) {
HRESULT hr = S_OK;
vector<float> cog_comp; // individual cognitive component
vector<float> soc_comp; // social component
cog_comp = vector_multiply(constriction_factor,
vector_addition(p.velocity,
vector_multiply(cognitive_weight*cognitive_random_factor,
vector_difference(p.best_sln,
p.curr_sln
))));
soc_comp = vector_multiply(social_weight*social_random_factor,
vector_difference(best_sln,
p.curr_sln
));
p.velocity = vector_addition(cog_comp, soc_comp);
return hr;
}
void spacecraft_collision(simulation_t* sim,spacecraft_t* spacecraft)
{
vector_t displacement=vector_subtract(spacecraft->base.position,spacecraft->orbit.primary->base.position);
double distance=vector_magnitude(displacement);
double penetration=spacecraft->orbit.primary->radius-distance;
if(penetration>0)
{
displacement=vector_multiply((distance+penetration)/distance,displacement);
spacecraft->base.position=vector_add(spacecraft->orbit.primary->base.position,displacement);
spacecraft->base.velocity=spacecraft->orbit.primary->base.velocity;
spacecraft->base.velocity.x+=spacecraft->orbit.primary->base.delta_rot*displacement.y;
spacecraft->base.velocity.y+=-spacecraft->orbit.primary->base.delta_rot*displacement.x;
spacecraft->base.rotation=atan2(-displacement.y,displacement.x)+M_PI/2;
spacecraft->base.delta_rot=0;
}
}
/* m1 x m2 */
Matrix *multiply(Matrix *m1, Matrix *m2){
Matrix *product, *trans;
unsigned int i, j;
if(m1 == NULL || m2 == NULL)
return NULL;
if(m1->columns != m2->rows)
return NULL;
trans = transpose(m1);
product = constructor(m1->rows, m2->columns);
for(i = 0; i < product->columns; i++){
for(j = 0; j < product->rows; j++){
product->numbers[i][j] = vector_multiply(trans->numbers[j], m2->numbers[i], product->rows);
}
}
destroy_matrix(trans);
return product;
}
/**
* Render a single ray from a camera.
* \param s Scene to render.
* \param r Ray being cast.
* \param wavelength
* \param depth How deep are we in the recursion?
* \return Energy of the ray.
*/
static float render_ray(const struct scene *s, struct ray *r, wavelength_t wavelength, int depth){
if(depth > MAX_DEPTH){
return 0;
}
struct object *obj = NULL;
MEASUREMENTS_RAY_SCENE_INTERSECTION();
float distance = kd_tree_ray_intersection(&(s->tree), r, 0, INFINITY, &obj);
if(isnan(distance)){
// If the ray didn't hit anything, it stays black.
return 0;
}
vector_t pointInCameraSpace = vector_add(r->origin, vector_multiply(r->direction, distance));
vector_t pointInObjectSpace = vector_transform(pointInCameraSpace, &(obj->invTransform));
vector_t normalInObjectSpace = obj->get_normal(obj, pointInObjectSpace);
vector_t normalInCameraSpace = vector_normalize(vector_transform_direction(normalInObjectSpace, &(obj->transform)));
#if DOT_PRODUCT_SHADING
(void)wavelength;
return fabsf(vector_dot(normalInCameraSpace, r->direction));
#else
float energy = 0;
/// \todo Surface properties shouldn't be in camera space.
if(object_is_light_source(obj)){
energy = (obj->light.energy)(pointInCameraSpace, wavelength, normalInCameraSpace, r->direction);
}
struct outgoing_direction sample =
(obj->surface.sample)(pointInCameraSpace, wavelength, normalInCameraSpace, r->direction);
struct ray newRay;
ray_from_direction(&newRay, pointInCameraSpace, sample.direction);
energy += sample.weight * render_ray(s, &newRay, wavelength, depth + 1);
return energy;
#endif
}
void test_vector()
{
Vector *v = vector_new(1,2,3);
CU_ASSERT(v->x == 1);
CU_ASSERT(v->y == 2);
CU_ASSERT(v->z == 3);
Vector *vc = vector_copy(v);
CU_ASSERT(vc->x == 1);
CU_ASSERT(vc->y == 2);
CU_ASSERT(vc->z == 3);
Vector *u = vector_new(4,5,6);
vector_add(v,u);
CU_ASSERT(v->x == 5);
CU_ASSERT(v->y == 7);
CU_ASSERT(v->z == 9);
vector_subtract(v,u);
CU_ASSERT(v->x == 1);
CU_ASSERT(v->y == 2);
CU_ASSERT(v->z == 3);
vector_multiply(v,2);
CU_ASSERT(v->x == 2);
CU_ASSERT(v->y == 4);
CU_ASSERT(v->z == 6);
Vector *w = vector_cross(v,u);
CU_ASSERT(w->x == -6);
CU_ASSERT(w->y == 12);
CU_ASSERT(w->z == -6);
vector_free(v);
vector_free(u);
vector_free(w);
v = vector_new(3,4,0);
u = vector_new(0,3,4);
CU_ASSERT(v->x==u->y);
CU_ASSERT(v->y==u->z);
w = vector_cross(v,u);
CU_ASSERT(w->x == 16);
CU_ASSERT(w->y == -12);
CU_ASSERT(w->z == 9);
CU_ASSERT(within_tol(vector_magnitude(v),5));
CU_ASSERT(within_tol(vector_magnitude(u),5));
vector_free(v);
vector_free(u);
vector_free(w);
v = vector_new(1,0,0);
u = vector_new(0,0,1);
double a = vector_angle(v,u)/D2R;
CU_ASSERT(within_tol(a,90));
CU_ASSERT(within_tol(vector_angle(u,v),90.*D2R));
w = vector_cross(v,u);
CU_ASSERT(within_tol(vector_magnitude(w),1));
CU_ASSERT(w->x==0);
CU_ASSERT(w->y==-1);
CU_ASSERT(w->z==0);
}
/**
* Start rendering a scene.
* \param s Scene to render.
* \param
* \param pixmap Pointer to pixel map with result.
* Must be large enough to hold #y0 - #y1 rows.
*/
void renderer_render(const struct scene *s, const struct renderer_chunk *chunk,
struct color *pixmap){
unsigned ymax = chunk->top + chunk->height;
// How many meters per pixel.
float inc = s->sensorWidth / (float)(s->width - 1);
float yy = inc * ((float)(s->height) / 2 - chunk->top);
float focus = s->focus / s->focalLength;
#if MEASUREMENTS_WITH_WARMUP
MEASUREMENTS_WARMUP();
#endif
MEASUREMENTS_START();
for(unsigned y = chunk->top; y < ymax; ++y){
float xx = - s->sensorWidth / 2;
for(unsigned x = 0; x < s->width; ++x){
color_black(pixmap);
for(unsigned i = 0; i < s->raysPerPx; ++i){
struct ray r;
// filmPoint = {
// -xx - random_number(0, inc),
// -yy - random_number(0, inc),
// -s->focalLength
// }
// focusPoint = - (lensCenter - filmPoint) * (s->focus / filmPoint.z)
vector_t focusPoint = vector_set(
focus * (xx + random_number(0, inc)),
focus * (yy + random_number(0, inc)),
s->focus);
vector_t lensPoint = vector_multiply(vector_random_in_circle(), s->apertureDiameter);
ray_from_points(&r, lensPoint, focusPoint);
#ifndef MEASUREMENTS_KD_TREE_STATS
struct photon p;
photon_random_init(&p);
p.energy = render_ray(s, &r, p.wavelength, 0);
photon_add_to_color(&p, pixmap);
#else
struct object *obj;
MEASUREMENTS_RAY_SCENE_INTERSECTION();
kd_tree_ray_intersection(&(s->tree), &r, &obj);
pixmap->r += measurementsObjectIntersectionCounter;
pixmap->g += measurementsTreeTraversalCounter;
#endif
}
color_scale(pixmap, 1.0f / s->raysPerPx);
//printf("x = %i y = %i pixmap->r = %.2f\n", x, y ,pixmap->r);
++pixmap;
xx += inc;
}
yy -= inc;
}
MEASUREMENTS_PRINT();
}
//// rotate ///////////////////////////////////////////////////////////////////
//
// rotate an img by given pitch, roll, yaw
// return error
//
uint32_t *
rotate(float roll,
float pitch,
float yaw,
uint32_t *in_img,
uint32_t *out_img)
{
//// init /////////////////////////////////////////////////////////////////
//
float lat, lng, nx, ny, nz, clt, slt, r, pi, tau;
uint32_t xx, yy, x, y, z, ilt, iln, h, w;
struct vector v, vr, vp, vy, ar, ap, ay, pix;
//
pi = 4.0*atan(1.0);
tau = pi*2;
//
///////////////////////////////////////////////////////////////////////////
//// size config //////////////////////////////////////////////////////////
//
// image height
h = 1792;
// image width
w = 3584;
// radius of sphere
r = 570.41131604135256;
//
///////////////////////////////////////////////////////////////////////////
//// angles to single vector //////////////////////////////////////////////
// see vector.c
//
// roll
vector_set(&ar, 1, 0, 0, roll);
vector_from_axis_angle(&vr, &ar);
//
// pitch
vector_set(&ap, 0, 1, 0, pitch);
vector_from_axis_angle(&vp, &ap);
//
// yaw
// if (yaw > 180) {
// yaw -= 360;
// }
//
vector_set(&ay, 0, 0, 1, yaw);
vector_from_axis_angle(&vy, &ay);
//
// combine quaternions into v
vector_multiply(&vp, &vy);
vector_multiply(&vr, &vp);
//
vector_clone(&vr, &v);
// vector_normalize(&v);
//
///////////////////////////////////////////////////////////////////////////
//// rotation /////////////////////////////////////////////////////////////
//
for (y = 0; y < h; y++) {
yy = y * w * 3;
// y to sphere latitude
lat = (float) y / h * pi;
// store the values
slt = sin(lat);
clt = cos(lat);
for (x = 0; x < w; x++) {
xx = x * 3;
// x to sphere longitude
lng = (float) x / w * tau - pi;
//// spherical to cartesian ///////////////////////////////////////
//
nx = r * slt * cos(lng);
ny = r * slt * sin(lng);
nz = r * clt;
//
///////////////////////////////////////////////////////////////////
//// rotate ///////////////////////////////////////////////////////
// see vector.c
//
// translate pixel into vector
vector_set(&pix, nx, ny, nz, 0);
// and rotate the pixel
vector_conjugate(&pix, &v);
// vector_print(&pix);
//
///////////////////////////////////////////////////////////////////
//// x,y,z -> lat,lng /////////////////////////////////////////////
lat = (float) acos(pix.z / r) / pi * h;
lng = (float) (atan2(pix.y, pix.x) + pi) / tau * w;
//
// pixel coords must be int
ilt = (int) lat;
iln = (int) lng;
//
// wrap latitude
if (ilt >= h) {
ilt -= h;
}
//
// wrap longitude
if (iln >= w) {
iln -= w;
}
//
///////////////////////////////////////////////////////////////////
for (z = 0; z < 3; z++) {
int pos = ilt * w * 3 + iln * 3 + z;
// pos > h * w * 3 + w * 3 + 3
if (pos > 19278339) {
printf("ERROR OVERFLOW");
return out_img;
}
out_img[yy + xx + z] = in_img[pos];
}
}
}
//
///////////////////////////////////////////////////////////////////////////
// we did it, we did it
return out_img;
}
