int SSBoneFrame_CheckTargetBoneLimit(struct ss_bone_frame_s *bf, struct ss_animation_s *ss_anim)
{
ss_bone_tag_p b_tag = b_tag = bf->bone_tags + ss_anim->targeting_bone;
float target_dir[3], target_local[3], limit_dir[3], t;
Mat4_vec3_mul_inv(target_local, bf->transform, ss_anim->target);
if(b_tag->parent)
{
Mat4_vec3_mul_inv(target_local, b_tag->parent->full_transform, target_local);
}
vec3_sub(target_dir, target_local, b_tag->transform + 12);
vec3_norm(target_dir, t);
vec3_copy(limit_dir, ss_anim->targeting_limit);
if((ss_anim->targeting_limit[3] == -1.0f) ||
(vec3_dot(limit_dir, target_dir) > ss_anim->targeting_limit[3]))
{
return 1;
}
return 0;
}
void GetRotationbyVector(mat33_t *R, const vec3_t *v1, const vec3_t *v2)
{
vec3_t diff;
vec3_sub_vec2(&diff, v1, v2);
if( vec3_norm(&diff) < 1e-3 ){
mat33_assign(R,1,0,0,0,1,0,0,0,1);
}else
{
if( fabs((vec3_norm(&diff)-2)) < 1e-3 ){
mat33_assign(R,1,0,0 ,0,-1,0, 0,0,-1);
}else
{
real_t winkel = _acos(vec3_dot(v1,v2));
quat_t QU;
vec3_t vc;
vec3_cross(&vc,v2,v1);
Quaternion_byAngleAndVector(&QU,winkel,&vc);
mat33_from_quat(R,&QU);
}
}
}
/**
* Component of the support function for a cylinder collider.
**/
static void
object_support_component_box(object_t *object,
float direction[3], float out[3])
{
float x = object->w / 2;
float y = object->h / 2;
float z = object->l / 2;
float trans[16];
float dot;
float max_val = -INFINITY;
size_t max;
size_t i;
float coords[8][3] = {
{ +x, +y, +z, },
{ +x, +y, -z, },
{ +x, -y, +z, },
{ +x, -y, -z, },
{ -x, +y, +z, },
{ -x, +y, -z, },
{ -x, -y, +z, },
{ -x, -y, -z, },
};
object_get_transform_mat(object, trans);
for (i = 0, max = 0; i < 8; i++) {
matrix_vec3_mul(trans, coords[i], 1, coords[i]);
dot = vec3_dot(direction, coords[i]);
if (dot <= max_val)
continue;
max_val = dot;
max = i;
}
memcpy(out, coords[max], 3 * sizeof(float));
}
pixel_data_t world_render(world_t *world, camera_t *camera) {
pixel_t **pixels = calloc(camera->h, sizeof(pixel_t *));
for (uint32_t i = 0; i < camera->h; i++) {
pixels[i] = malloc(sizeof(pixel_t) * camera->w);
}
pixel_data_t pixel_data = { camera->w, camera->h, pixels };
for (uint32_t x = 0; x < camera->w; x++) {
for (uint32_t y = 0; y < camera->h; y++) {
ray_t *ray = ray_new(camera->pos, dir_for_pixel(camera, x, y));
sphere_t *sphere = ptr_array_index(world->spheres, 0);
vec3_t normal;
double distance;
if (!object_get_collision(OBJECT(sphere), ray, &distance, &normal))
continue;
vec3_t p = vec3_add(ray->pos, vec3_mul(ray->dir, distance));
pixel_t pixel = { 1, 0, 0, 0 };
for (uint32_t i = 0; i < world->lights->len; i++) {
light_t *light = ptr_array_index(world->lights, i);
vec3_t l = vec3_normalize(vec3_sub(p, light->pos));
double lambert = MAX(0, vec3_dot(l, normal) * LAMBERT_COEF);
pixel.a = 1;
pixel.r += 1 * lambert;
pixel.g += 1 * lambert;
pixel.b += 1 * lambert;
}
pixels[x][y] = pixel;
}
}
return pixel_data;
}
void CDynamicBSP::AddBSPPolygon(struct bsp_node_s *leaf, struct polygon_s *p)
{
if(m_realloc_state || (m_vertex_allocated + p->vertex_count >= m_vertex_buffer_size))
{
m_realloc_state = NEED_REALLOC_VERTEX_BUFF;
return;
}
bsp_polygon_p bp = (bsp_polygon_p)(m_tree_buffer + m_tree_allocated);
m_tree_allocated += sizeof(bsp_polygon_t);
bp->texture_index = p->texture_index;
bp->transparency = p->transparency;
bp->vertex_count = p->vertex_count;
bp->indexes = (GLuint*)(m_tree_buffer + m_tree_allocated);
m_tree_allocated += p->vertex_count * sizeof(GLint);
//vertex_p v = m_vertex_buffer + m_vertex_allocated;
//vertex_p pv = p->vertices;
memcpy(m_vertex_buffer + m_vertex_allocated, p->vertices, p->vertex_count * sizeof(vertex_t));
for(uint16_t i = 0; i < p->vertex_count; i++/*, v++, pv++*/)
{
bp->indexes[i] = m_vertex_allocated++;
//*v = *pv;
}
if(vec3_dot(p->plane, leaf->plane) > 0.9)
{
bp->next = leaf->polygons_front;
leaf->polygons_front = bp;
}
else
{
bp->next = leaf->polygons_back;
leaf->polygons_back = bp;
}
m_added_polygons++;
}
bool sphere::intersect(const ray& ray, const range& r, surface_hit* hit)
{
float t = sphere_intersect(center, radius, ray, r);
if(t == NO_t)
return false;
if(t > hit->t)
return false;
if(keep_stats) sphere_shadings++;
vec3 point = vec3_add(ray.o, vec3_scale(ray.d, t));
// snap to sphere surface
vec3 to_surface = vec3_subtract(point, center);
float distance = sqrtf(vec3_dot(to_surface, to_surface));
hit->point = vec3_add(center, vec3_scale(to_surface, radius / distance));
hit->normal = vec3_divide(to_surface, radius);
hit->color = color;
hit->t = t;
return true;
}
enum cull_result box_cull(const box_t *box, const plane_t *planes, int nplanes)
{
int visible = 0;
for(int i = 0; i < nplanes; i++) {
const plane_t *p = &planes[i];
float reff =
fabsf(box->extent.x * p->normal.x) +
fabsf(box->extent.y * p->normal.y) +
fabsf(box->extent.z * p->normal.z);
float dot = vec3_dot(&p->normal, &box->centre) + p->dist;
if (dot <= -reff)
return CULL_OUT;
else {
if (++visible == nplanes)
return CULL_IN;
}
}
return CULL_PARTIAL;
}
float vec3_magnitude(const vec3_t *v)
{
return sqrtf(vec3_dot(v, v));
}
void Calculate_Obs(double time, const double pos[3], const double vel[3], geodetic_t *geodetic, vector_t *obs_set)
{
/* The procedures Calculate_Obs and Calculate_RADec calculate */
/* the *topocentric* coordinates of the object with ECI position, */
/* {pos}, and velocity, {vel}, from location {geodetic} at {time}. */
/* The {obs_set} returned for Calculate_Obs consists of azimuth, */
/* elevation, range, and range rate (in that order) with units of */
/* radians, radians, kilometers, and kilometers/second, respectively. */
/* The WGS '72 geoid is used and the effect of atmospheric refraction */
/* (under standard temperature and pressure) is incorporated into the */
/* elevation calculation; the effect of atmospheric refraction on */
/* range and range rate has not yet been quantified. */
/* The {obs_set} for Calculate_RADec consists of right ascension and */
/* declination (in that order) in radians. Again, calculations are */
/* based on *topocentric* position using the WGS '72 geoid and */
/* incorporating atmospheric refraction. */
double sin_lat, cos_lat, sin_theta, cos_theta, el, azim, top_s, top_e, top_z;
double obs_pos[3];
double obs_vel[3];
double range[3];
double rgvel[3];
Calculate_User_PosVel(time, geodetic, obs_pos, obs_vel);
vec3_sub(pos, obs_pos, range);
vec3_sub(vel, obs_vel, rgvel);
double range_length = vec3_length(range);
sin_lat=sin(geodetic->lat);
cos_lat=cos(geodetic->lat);
sin_theta=sin(geodetic->theta);
cos_theta=cos(geodetic->theta);
top_s=sin_lat*cos_theta*range[0]+sin_lat*sin_theta*range[1]-cos_lat*range[2];
top_e=-sin_theta*range[0]+cos_theta*range[1];
top_z=cos_lat*cos_theta*range[0]+cos_lat*sin_theta*range[1]+sin_lat*range[2];
azim=atan(-top_e/top_s); /* Azimuth */
if (top_s>0.0)
azim=azim+pi;
if (azim<0.0)
azim = azim + 2*M_PI;
el=ArcSin(top_z/range_length);
obs_set->x=azim; /* Azimuth (radians) */
obs_set->y=el; /* Elevation (radians) */
obs_set->z=range_length; /* Range (kilometers) */
/* Range Rate (kilometers/second) */
obs_set->w = vec3_dot(range, rgvel)/vec3_length(range);
obs_set->y=el;
/**** End bypass ****/
if (obs_set->y<0.0)
obs_set->y=el; /* Reset to true elevation */
}
/**
* Check for collision between objects.
**/
int
object_check_collision(object_t *a, object_t *b)
{
float direction[3] = { 0, 1, 0 };
float simplex[4][3];
size_t simplex_pos = 5;
size_t last;
float middle[3];
float to_last[3];
if (! (object_is_collider(a) &&
object_is_collider(b)))
return 0;
object_support(a, b, direction, simplex[0]);
direction[2] = -1;
object_support(a, b, direction, simplex[1]);
if (vec3_dot(simplex[1], direction) <= 0)
return 0;
vec3_subtract(simplex[1], simplex[0], to_last);
vec3_cross(simplex[1], to_last, direction);
vec3_cross(direction, to_last, direction);
object_support(a, b, direction, simplex[2]);
if (vec3_dot(simplex[2], direction) <= 0)
return 0;
while (vec3_dot(simplex[simplex_pos], direction)) {
if (simplex_pos == 5) {
simplex_pos = 3;
last = 2;
} else {
last = simplex_pos;
simplex_pos =
object_simplex_detect_region(simplex,
simplex_pos);
}
if (simplex_pos == 4)
return 1;
middle[0] = middle[1] = middle[2] = 0;
if (simplex_pos != 3)
vec3_add(simplex[3], middle, middle);
if (simplex_pos != 2)
vec3_add(simplex[2], middle, middle);
if (simplex_pos != 1)
vec3_add(simplex[1], middle, middle);
if (simplex_pos != 0)
vec3_add(simplex[0], middle, middle);
vec3_scale(middle, middle, 1.0/3.0);
vec3_subtract(middle, simplex[last], to_last);
vec3_cross(middle, to_last, direction);
vec3_cross(direction, to_last, direction);
object_support(a, b, direction, simplex[simplex_pos]);
}
return 0;
}
Vec3 vec3_reflect(Vec3* vIncidentVector, Vec3* vNormalVector) {
return Vec3_diff(*vIncidentVector, vec3_mult(*vNormalVector, 2.0f * vec3_dot(vNormalVector, vIncidentVector)));
}
int scene_half_space_is_point_inside_solid(const Scene* scene, const Vec3* point) {
const Plane* plane = (const Plane*)scene->data;
return vec3_dot(point, &plane->n) + plane->d <= 0;
}
void fragmentShader(float x, float y) {
if (!cgInsideZBuffer(x, y)) {
return;
}
float w1 = m_ctx.WW1Y * x + m_ctx.WW1X * y + m_ctx.W1A;
float w2 = m_ctx.WW2Y * x + m_ctx.WW2X * y + m_ctx.W2A;
float w3 = 1.0f - w1 - w2;
m_ctx.w.x = w1 * m_ctx.ONE_DIV_Z1;
m_ctx.w.y = w2 * m_ctx.ONE_DIV_Z2;
m_ctx.w.z = w3 * m_ctx.ONE_DIV_Z3;
float z = m_ctx.w.w = m_ctx.w.x + m_ctx.w.y + m_ctx.w.z;
float zBuff = cgGetZBuffer(x, y);
if (z > zBuff) {
// Aplicar pesos
weight(&m_ctx.v1.vVaryingNormal, &m_ctx.v2.vVaryingNormal, &m_ctx.v3.vVaryingNormal, &m_ctx.vVaryingNormal);
weight(&m_ctx.v1.vEyeVec, &m_ctx.v2.vEyeVec, &m_ctx.v3.vEyeVec, &m_ctx.vEyeVec);
vec3_normalize(&m_ctx.vVaryingNormal);
vec3_normalize(&m_ctx.vEyeVec);
Vec4 final_color;
final_color.x = 0;
final_color.y = 0;
final_color.z = 0;
final_color.w = 1;
for (int i = 0; i < m_ctx.vPointLights; i++) {
weight(&m_ctx.v1.vPointLightsDir[i], &m_ctx.v2.vPointLightsDir[i], &m_ctx.v3.vPointLightsDir[i], &m_ctx.vPointLightsDir[i]);
vec3_normalize(&m_ctx.vPointLightsDir[i]);
float lambertTerm = vec3_dot(&m_ctx.vVaryingNormal, &m_ctx.vPointLightsDir[i]);
if (lambertTerm > 0.0) {
Vec4 diffuse = vec4_mult(m_ctx.diffuseColor, lambertTerm);
final_color.x += diffuse.x;
final_color.y += diffuse.y;
final_color.z += diffuse.z;
Vec3 vNegLightDir;
vNegLightDir.x = m_ctx.vPointLightsDir[i].x;
vNegLightDir.y = m_ctx.vPointLightsDir[i].y;
vNegLightDir.z = m_ctx.vPointLightsDir[i].z;
Vec3 E = m_ctx.vEyeVec;
Vec3 R = vec3_reflect(&vNegLightDir, &m_ctx.vVaryingNormal);
float _spec = vec3_dot(&R, &E);
float spec = pow(max(0.0f, _spec), m_ctx.specularCoef);
Vec4 specular = vec4_mult(m_ctx.specularColor, spec);
final_color.x += specular.x;
final_color.y += specular.y;
final_color.z += specular.z;
}
}
for (int i = 0; i < m_ctx.vDirectLights; i++) {
weight(&m_ctx.v1.vDirectLightsDir[i], &m_ctx.v2.vDirectLightsDir[i], &m_ctx.v3.vDirectLightsDir[i], &m_ctx.vDirectLightsDir[i]);
vec3_normalize(&m_ctx.vDirectLightsDir[i]);
float lambertTerm = vec3_dot(&m_ctx.vVaryingNormal, &m_ctx.vDirectLightsDir[i]);
if (lambertTerm > 0.0) {
Vec4 diffuse = vec4_mult(m_ctx.diffuseColor, lambertTerm);
final_color.x += diffuse.x;
final_color.y += diffuse.y;
final_color.z += diffuse.z;
Vec3 vNegLightDir;
vNegLightDir.x = m_ctx.vDirectLightsDir[i].x;
vNegLightDir.y = m_ctx.vDirectLightsDir[i].y;
vNegLightDir.z = m_ctx.vDirectLightsDir[i].z;
Vec3 E = m_ctx.vEyeVec;
Vec3 R = vec3_reflect(&vNegLightDir, &m_ctx.vVaryingNormal);
float _spec = vec3_dot(&R, &E);
float spec = pow(max(0.0f, _spec), m_ctx.specularCoef);
Vec4 specular = vec4_mult(m_ctx.specularColor, spec);
final_color.x += specular.x;
final_color.y += specular.y;
final_color.z += specular.z;
}
}
// Aplicar color
float r = (255 * final_color.x);
float g = (255 * final_color.y);
float b = (255 * final_color.z);
Color c;
// Wireframe
// float w = min3(m_ctx.w.x, m_ctx.w.y, m_ctx.w.z) / m_ctx.w.w;
// if (w < 0.02f) {
// w /= 0.02f;
// r *= w;
// g *= w;
// b *= w;
// }
c.r = clamp(r, 0, 255);
c.b = clamp(g, 0, 255);
c.g = clamp(b, 0, 255);
cg_putpixel(x, y, c);
cgPutZBuffer(x, y, z);
} else {
m_ctx.zbuffered++;
}
}
float Vector::__mul__(const Vector& v1)
{
return vec3_dot(&this->v_, &v1.v_);
}
Vec3 vec3_reflect(Vec3 d, Vec3 n)
{
/* d - 2(d.n)n */
return vec3_add(d, vec3_scale(-2*vec3_dot(d,n),n));
}
void gfx_csm_prepare(const struct gfx_view_params* params, const struct vec3f* light_dir,
const struct aabb* world_bounds)
{
static const float tolerance = 10.0f;
struct vec3f dir;
vec3_setv(&dir, light_dir);
float texoffset_x = 0.5f + (0.5f/g_csm->shadowmap_size);
float texoffset_y = 0.5f + (0.5f/g_csm->shadowmap_size);
struct mat3f view_inv;
struct mat4f tex_mat;
struct mat4f tmp_mat;
float splits[CSM_CASCADE_CNT+1];
mat3_setf(&view_inv,
params->view.m11, params->view.m21, params->view.m31,
params->view.m12, params->view.m22, params->view.m32,
params->view.m13, params->view.m23, params->view.m33,
params->cam_pos.x, params->cam_pos.y, params->cam_pos.z);
mat4_setf(&tex_mat,
0.5f, 0.0f, 0.0f, 0.0f,
0.0f, -0.5f, 0.0f, 0.0f,
0.0f, 0.0f, 1.0f, 0.0f,
texoffset_x, texoffset_y, 0.0f, 1.0f);
float csm_far = minf(CSM_FAR_MAX, params->cam->ffar);
csm_split_range(params->cam->fnear, csm_far, splits);
/* calculate cascades */
struct frustum f; /* frustum points for cascades */
struct plane vp_planes[6];
for (uint i = 0; i < CSM_CASCADE_CNT; i++) {
cam_calc_frustumcorners(params->cam, (struct vec3f*)f.points, &splits[i], &splits[i+1]);
csm_calc_minsphere(&g_csm->cascades[i].bounds, &f, ¶ms->view, &view_inv);
memcpy(&g_csm->cascade_frusts[i], &f, sizeof(f));
/* cascade matrixes: first we find two extreme points of the world, related to cascade */
struct vec3f scenter;
struct ray r;
struct plane upper_plane;
struct plane lower_plane;
struct vec3f upper_pt;
struct vec3f lower_pt;
struct vec3f xaxis;
struct vec3f yaxis;
struct vec3f viewpos;
struct vec3f tmp;
struct vec3f lowerpt_vs;
float sr = g_csm->cascades[i].bounds.r;
vec3_setf(&scenter, g_csm->cascades[i].bounds.x, g_csm->cascades[i].bounds.y,
g_csm->cascades[i].bounds.z);
ray_setv(&r, &scenter, &dir);
plane_setv(&upper_plane, &g_vec3_unity_neg, fabs(world_bounds->maxpt.y));
plane_setv(&lower_plane, &g_vec3_unity, fabs(world_bounds->minpt.y));
vec3_sub(&upper_pt, &r.pt,
vec3_muls(&tmp, &dir, fabs(ray_intersect_plane(&r, &upper_plane)) + tolerance));
vec3_add(&lower_pt, &r.pt,
vec3_muls(&tmp, &dir, fabs(ray_intersect_plane(&r, &lower_plane)) + tolerance));
/* view matrix of light view for the cascade :
* dir = light_dir
* up = (1, 0, 0)
* pos = sphere center
*/
vec3_norm(&xaxis, vec3_cross(&tmp, &g_vec3_unitx, &dir));
vec3_cross(&yaxis, &dir, &xaxis);
vec3_sub(&viewpos, &upper_pt, vec3_muls(&tmp, &dir, tolerance));
mat3_setf(&g_csm->cascades[i].view,
xaxis.x, yaxis.x, dir.x,
xaxis.y, yaxis.y, dir.y,
xaxis.z, yaxis.z, dir.z,
-vec3_dot(&xaxis, &viewpos), -vec3_dot(&yaxis, &viewpos), -vec3_dot(&dir, &viewpos));
/* orthographic projection matrix for cascade */
vec3_transformsrt(&lowerpt_vs, &lower_pt, &g_csm->cascades[i].view);
float nnear = tolerance*0.5f;
float nfar = lowerpt_vs.z + sr;
csm_calc_orthoproj(&g_csm->cascades[i].proj,
sr*2.0f + 1.0f/g_csm->shadowmap_size,
sr*2.0f + 1.0f/g_csm->shadowmap_size,
nnear, nfar);
g_csm->cascades[i].nnear = nnear;
g_csm->cascades[i].nfar = nfar;
/* calculate final matrix */
mat3_mul4(&g_csm->cascade_vps[i], &g_csm->cascades[i].view, &g_csm->cascades[i].proj);
cam_calc_frustumplanes(vp_planes, &g_csm->cascade_vps[i]);
csm_calc_cascadeplanes(&g_csm->cascade_planes[i*4], vp_planes, &g_csm->cascade_vps[i]);
csm_round_mat(&g_csm->cascade_vps[i], &g_csm->cascade_vps[i], g_csm->shadowmap_size);
mat4_mul(&g_csm->shadow_mats[i],
mat3_mul4(&tmp_mat, &view_inv, &g_csm->cascade_vps[i]), &tex_mat);
}
/* caculate shadow area bounds (aabb) */
struct aabb cascade_near;
struct aabb cascade_far;
aabb_setzero(&g_csm->frustum_bounds);
aabb_from_sphere(&cascade_near, &g_csm->cascades[0].bounds);
aabb_from_sphere(&cascade_far, &g_csm->cascades[CSM_CASCADE_CNT-1].bounds);
aabb_merge(&g_csm->frustum_bounds, &cascade_near, &cascade_far);
vec3_setv(&g_csm->light_dir, &dir);
}
// ===========================================================================================
void abskernel(mat33_t *R, vec3_t *t, vec3_t *Qout, real_t *err2,
const vec3_t *_P, const vec3_t *_Q,
const mat33_t *F, const mat33_t *G, const int n)
{
int i, j;
vec3_t P[n];//(_P.begin(),_P.end());
vec3_t Q[n];//(_Q.begin(),_Q.end());
//const unsigned int n = (unsigned int) P.size();
memcpy(&*P, _P, n*sizeof(vec3_t));
memcpy(&*Q, _Q, n*sizeof(vec3_t));
for(i=0; i<n; i++)
{
vec3_t _q;
vec3_mult_mat(&_q, &F[i], &_Q[i]);
vec3_copy(&Q[i], &_q);
}
vec3_t pbar;
vec3_array_sum(&pbar, &*P, n);
vec3_div(&pbar, (real_t)(n));
vec3_array_sub(&*P, &pbar, n);
vec3_t qbar;
vec3_array_sum(&qbar, &*Q, n);
vec3_div(&qbar, (real_t)(n));
vec3_array_sub(&*Q, &qbar, n);
mat33_t M;
mat33_clear(&M);
for(j=0; j<n; j++)
{
mat33_t _m;
vec3_mul_vec3trans(&_m, &P[j], &Q[j]);
mat33_add(&M, &_m);
}
mat33_t _U;
mat33_t _S;
mat33_t _V;
mat33_clear(&_U);
mat33_clear(&_S);
mat33_clear(&_V);
mat33_svd2(&_U, &_S, &_V, &M);
mat33_t _Ut;
mat33_transpose(&_Ut, _U);
mat33_mult_mat2(R, &_V, &_Ut);
// R = V*(U.');
// WE need to check the determinant of the R
real_t det = mat33_det(R);
if ( det < 0 )
{
mat33_t Vc;
Vc.m[0] = _V.m[0]; Vc.m[1] = _V.m[1]; Vc.m[2] = -_V.m[2];
Vc.m[3] = _V.m[3]; Vc.m[4] = _V.m[4]; Vc.m[5] = -_V.m[5];
Vc.m[6] = _V.m[6]; Vc.m[7] = _V.m[7]; Vc.m[8] = -_V.m[8];
mat33_mult_mat2(R, &Vc, &_Ut); // to have det(R) == 1
optimal_t(t, R, G, F, &*P, n);
if( t->v[2] < 0 ){
Vc.m[0] =- _V.m[0]; Vc.m[1] =- _V.m[1]; Vc.m[2] = -_V.m[2];
Vc.m[3] =- _V.m[3]; Vc.m[4] =- _V.m[4]; Vc.m[5] = -_V.m[5];
Vc.m[6] =- _V.m[6]; Vc.m[7] =- _V.m[7]; Vc.m[8] = -_V.m[8];
mat33_mult_mat2(R, &Vc, &_Ut); // to have det(R) == 1 & t_3 > 0
optimal_t(t, R, G, F, &*P, n);
}
}else{
optimal_t(t,R,G,F,&*P,n);
if( t->v[2] < 0 )
{
mat33_t Vc;
Vc.m[0] = -_V.m[0]; Vc.m[1] = -_V.m[1]; Vc.m[2] = _V.m[2];
Vc.m[3] = -_V.m[3]; Vc.m[4] = -_V.m[4]; Vc.m[5] = _V.m[5];
Vc.m[6] = -_V.m[6]; Vc.m[7] = -_V.m[7]; Vc.m[8] = _V.m[8];
mat33_mult_mat2(R, &Vc, &_Ut);
optimal_t(t,R,G,F,&*P,n);
}
}
// CHECK if everything is working ok
// check the det of R
//det = mat33_det(R);
xform(Qout,&*P,R,t, n);
*err2 = 0.0;
mat33_t _m1;
vec3_t _v1;
for(i=0; i<n; i++)
{
mat33_eye(&_m1);
mat33_sub(&_m1, &F[i]);
vec3_mult_mat(&_v1, &_m1, &Qout[i]);
*err2 += vec3_dot(&_v1, &_v1);
}
}
void CDynamicBSP::AddNewPolygonList(struct polygon_s *p, float transform[16], struct frustum_s *f)
{
for( ; p && (!m_realloc_state); p = p->next)
{
m_temp_allocated = 0;
polygon_p np = this->CreatePolygon(p->vertex_count);
bool visible = (f == NULL);
vertex_p src_v, dst_v;
np->anim_id = p->anim_id;
np->frame_offset = p->frame_offset;
np->double_side = p->double_side;
np->transparency = p->transparency;
Mat4_vec3_rot_macro(np->plane, transform, p->plane);
for(uint16_t i = 0; i < p->vertex_count; i++)
{
src_v = p->vertices + i;
dst_v = np->vertices + i;
Mat4_vec3_mul_macro(dst_v->position, transform, src_v->position);
}
np->plane[3] = -vec3_dot(np->plane, np->vertices[0].position);
for(frustum_p ff = f; (!visible) && ff; ff = ff->next)
{
if(Frustum_IsPolyVisible(np, ff, false))
{
visible = true;
break;
}
}
if(visible)
{
if(p->anim_id > 0)
{
anim_seq_p seq = m_anim_seq + p->anim_id - 1;
uint16_t frame = (seq->current_frame + p->frame_offset) % seq->frames_count;
tex_frame_p tf = seq->frames + frame;
np->texture_index = tf->texture_index;
for(uint16_t i = 0; i < p->vertex_count; i++)
{
src_v = p->vertices + i;
dst_v = np->vertices + i;
Mat4_vec3_rot_macro(dst_v->normal, transform, src_v->normal);
vec4_copy(dst_v->color, src_v->color);
ApplyAnimTextureTransformation(dst_v->tex_coord, src_v->tex_coord, tf);
}
}
else
{
np->texture_index = p->texture_index;
for(uint16_t i = 0; i < p->vertex_count; i++)
{
src_v = p->vertices + i;
dst_v = np->vertices + i;
Mat4_vec3_rot_macro(dst_v->normal, transform, src_v->normal);
vec4_copy(dst_v->color, src_v->color);
dst_v->tex_coord[0] = src_v->tex_coord[0];
dst_v->tex_coord[1] = src_v->tex_coord[1];
}
}
m_input_polygons++;
this->AddPolygon(m_root, np);
}
}
}
static bool ray_kd_tree_intersect(Ray ray, const Vec3 *vertex_list,
const KdNode *node, struct TriangleHit *hit)
{
const Vec3 plane_normal[3] =
{(Vec3) {1, 0, 0}, (Vec3) {0, 1, 0}, (Vec3) {0, 0, 1}};
KdNode *node_near, *node_far;
struct TriangleHit hit_near, hit_far;
bool did_near, did_far;
Ray ray_near = ray, ray_far = ray;
double clip_t;
/* In a leaf we have to check all triangles */
if (node->leaf)
return ray_kd_leaf_intersect(ray, vertex_list, node, hit);
switch(node->axis)
{
case X_AXIS:
clip_t = (node->location - ray.origin.x)/ray.direction.x;
break;
case Y_AXIS:
clip_t = (node->location - ray.origin.y)/ray.direction.y;
break;
case Z_AXIS:
clip_t = (node->location - ray.origin.z)/ray.direction.z;
break;
default:
printf("Invalid axis %d\n", node->axis);
exit(1);
break;
}
if (vec3_dot(ray.direction, plane_normal[node->axis]) > 0.0)
{
node_near = node->left;
node_far = node->right;
} else
{
node_near = node->right;
node_far = node->left;
}
/* The major performance improvement from using kd-trees
* Don't check a branch of a tree if the ray can't possibly intersect it */
if (clip_t > ray.far)
return ray_kd_tree_intersect(ray, vertex_list, node_near, hit);
if (clip_t < ray.near)
return ray_kd_tree_intersect(ray, vertex_list, node_far, hit);
/* Split the ray in twain */
ray_near.near = ray.near; ray_near.far = clip_t;
ray_far.near = clip_t; ray_far.far = ray.far;
/* The invariant of a kd-tree is that every point in the near node will
* be closer than any point in the far node. So we start by checking the
* near node and if we find an intersection inside it, we don't check the
* far node anymore as it can't possible contain a closer intersection. */
did_near = ray_kd_tree_intersect(ray_near, vertex_list, node_near, &hit_near);
/* The test (hit_near.t < clip_t) is important, as it is possible a
* primitive in the far node will intersect closer than this primitive,
* which lies only partially in the near node. */
if (did_near && hit_near.t <= clip_t)
{
*hit = hit_near;
return true;
}
did_far = ray_kd_tree_intersect(ray_far, vertex_list, node_far, &hit_far);
*hit = hit_far;
return did_far;
}
float vec3_length2 (const vec3 v1) {
return vec3_dot(v1, v1);
}
group* make_tree(sphere* spheres, int start, unsigned int count, int level = 0)
{
if(level == 0) {
previous = time(NULL);
if(getenv("DEBUG_BVH") != NULL) {
bvh_spheres_debug_output = fopen("bvh.r9", "w");
}
}
if(time(NULL) > previous) {
fprintf(stderr, "total treed = %d\n", total_treed);
previous = time(NULL);
}
if((level >= bvh_max_depth) || count <= bvh_leaf_max) {
total_treed += count;
group* g = new group(spheres, start, count);
if(bvh_spheres_debug_output != NULL) {
// fprintf(bvh_spheres_debug_output, "sphere 7 %f %f %f %f 1 1 1 # %d\n", g->radius, g->center.x, g->center.y, g->center.z, level );
if(level == 0) {
fclose(bvh_spheres_debug_output);
bvh_spheres_debug_output = NULL;
}
}
bvh_leaf_size_counts[std::min(63U, count)]++;
bvh_leaf_count++;
bvh_level_counts[level]++;
bvh_node_count++;
return g;
}
vec3 split_pivot;
vec3 split_plane_normal;
// find bounding box
vec3 boxmin, boxmax;
box_init(boxmin, boxmax);
for(unsigned int i = 0; i < count; i++) {
sphere &s = spheres[start + i];
vec3 spheremin = vec3_subtract(s.center, vec3(s.radius));
vec3 spheremax = vec3_add(s.center, vec3(s.radius));
box_extend(boxmin, boxmax, spheremin, spheremax);
}
split_pivot = vec3_scale(vec3_add(boxmin, boxmax), .5);
vec3 boxdim = vec3_subtract(boxmax, boxmin);
if(boxdim.x > boxdim.y && boxdim.x > boxdim.z) {
split_plane_normal = vec3(1, 0, 0);
} else if(boxdim.y > boxdim.z) {
split_plane_normal = vec3(0, 1, 0);
} else {
split_plane_normal = vec3(0, 0, 1);
}
// XXX output split plane to BVH debug file
int startA;
int countA;
int startB;
int countB;
if(!bvh_split_median) {
int s1 = start - 1;
int s2 = start + count;
do {
// from start to s1, not including s1, is negative
// from s2 to start + count - 1 is positive
do {
s1 += 1;
} while((s1 < s2) && vec3_dot(vec3_subtract(spheres[s1].center, split_pivot), split_plane_normal) < 0);
// If there wasn't a positive sphere before s2, done.
if(s1 >= s2)
break;
// s1 is now location of lowest positive sphere
do {
s2 -= 1;
} while((s1 < s2) && vec3_dot(vec3_subtract(spheres[s2].center, split_pivot), split_plane_normal) >= 0);
// If there wasn't a negative sphere between s1 and s2, done
if(s1 >= s2)
break;
// s2 is now location of highest negative sphere
std::swap(spheres[s1], spheres[s2]);
} while(true);
// s1 is the first of the positive spheres
startA = start;
countA = s1 - startA;
startB = s1;
countB = start + count - s1;
} else {
sphere_sorter sorter(split_plane_normal);
std::sort(spheres + start, spheres + start + count - 1, sorter);
startA = start;
countA = count / 2;
startB = startA + countA;
countB = count - countA;
}
group *g;
if(countA > 0 && countB > 0) {
// get a tighter bound around children than hierarchical bounds
vec3 boxmin, boxmax;
box_init(boxmin, boxmax);
for(unsigned int i = 0; i < count; i++) {
add_sphere(&boxmin, &boxmax, spheres[start + i].center, spheres[start + i].radius);
}
// construct children
group *g1 = make_tree(spheres, startA, countA, level + 1);
g = new group(spheres, g1, NULL, split_plane_normal, boxmin, boxmax);
group *g2 = make_tree(spheres, startB, countB, level + 1);
g->positive = g2;
bvh_level_counts[level]++;
bvh_node_count++;
} else {
total_treed += count;
fprintf(stderr, "Leaf node at %d, %u spheres, total %d\n", level, count, total_treed);
g = new group(spheres, start, count);
bvh_leaf_size_counts[std::min(63U, count)]++;
bvh_leaf_count++;
bvh_level_counts[level]++;
bvh_node_count++;
}
if(bvh_spheres_debug_output != NULL) {
// fprintf(bvh_spheres_debug_output, "sphere 7 %f %f %f %f 1 1 1 # %d\n", g->radius, g->center.x, g->center.y, g->center.z, level );
if(level == 0) {
fclose(bvh_spheres_debug_output);
bvh_spheres_debug_output = NULL;
}
}
return g;
}
static int _llfunc_vec3_dot(lua_State *L) {
vec3 *a = (vec3*)userdata_get_or_die(L, 1);
vec3 *b = (vec3*)userdata_get_or_die(L, 2);
lua_pushnumber(L, vec3_dot(a, b));
return 1;
}
world *load_world(char *fname) // Get world and return pointer.
{
char *inpstr;
int wkd;
std::auto_ptr<world> w(new world);
time_t prev = time(NULL);
scoped_FILE fp(fopen(fname, "r"));
if(fp == NULL) {
fprintf(stderr, "Cannot open file %s for input.\nE#%d\n", fname, errno);
return NULL;
}
if((inpstr = getstr(fp)) == NULL) {
fprintf(stderr, "Cannot read title.\n");
return NULL;
}
if(strcmp(inpstr, ".") != 0) {
w->Title = NULL;
} else {
w->Title = new char[strlen(inpstr) + 1];
strcpy(w->Title, inpstr);
}
// lace now ignored
int lace;
if(!getint(fp, &lace)) {
fprintf(stderr, "*!LACE\n");
return NULL;
}
int wdth, lnth; // now ignored
if(!getint(fp, &wdth)) {
fprintf(stderr, "*!WDTH\n");
return NULL;
}
if(!getint(fp, &lnth)) {
fprintf(stderr, "*!LNTH\n");
return NULL;
}
int xsub, ysub; // now ignored
if(!getint(fp, &xsub)) {
fprintf(stderr, "*!xsub\n");
return NULL;
}
if(!getint(fp, &ysub)) {
fprintf(stderr, "*!ysub\n");
return NULL;
}
w->xsub = 1;
w->ysub = 1;
int r, g, b, brt;
if(!getint(fp, &r)) {
fprintf(stderr, "*!backgroundr\n");
return NULL;
}
if(!getint(fp, &g)) {
fprintf(stderr, "*!backgroundg\n");
return NULL;
}
if(!getint(fp, &b)) {
fprintf(stderr, "*!backgroundb\n");
return NULL;
}
if(!getint(fp, &brt)) {
fprintf(stderr, "*!backgroundb\n");
return NULL;
}
w->background.x = r / 16.0 * brt / 100.0;
w->background.y = g / 16.0 * brt / 100.0;
w->background.z = b / 16.0 * brt / 100.0;
int df;
if(!getint(fp, &df)) {
fprintf(stderr, "*!DIFFUSION\n");
return NULL;
}
if(!getint(fp, &w->sphere_count)) {
fprintf(stderr, "*!#Sphere\n");
return NULL;
}
prev = time(NULL);
w->spheres = new sphere[w->sphere_count];
for(int i = 0; i < w->sphere_count; i++) {
if(time(NULL) > prev) {
prev = time(NULL);
fprintf(stderr, "loaded %u spheres\n", i);
}
float x, y, z, radius;
wkd = fltget(fp, &x) && fltget(fp, &y);
wkd = (wkd && fltget(fp, &z) && fltget(fp, &radius));
int r, g, b, brt;
wkd = (wkd && getint(fp, &r) && getint(fp, &g));
wkd = (wkd && getint(fp, &b) && getint(fp, &brt));
if(!wkd) {
fprintf(stderr, "*!Sphere #%d\n", i);
return NULL;
}
vec3 color(r / 16.0 * brt / 100.0,
g / 16.0 * brt / 100.0, b / 16.0 * brt / 100.0);
w->spheres[i] = sphere(vec3(x, y, z), radius, color);
}
w->scene_center = w->spheres[0].center;
for(int i = 1; i < w->sphere_count; i++) {
w->scene_center = vec3_add(w->spheres[i].center, w->scene_center);
}
w->scene_center = vec3_divide(w->scene_center, w->sphere_count);
w->scene_extent = 0;
for(int i = 0; i < w->sphere_count; i++) {
vec3 to_center = vec3_subtract(w->scene_center, w->spheres[i].center);
float distance = sqrtf(vec3_dot(to_center, to_center)) + w->spheres[i].radius;
w->scene_extent = std::max(w->scene_extent, distance);
}
w->scene_extent *= 2;
w->root = make_tree(w->spheres, 0, w->sphere_count);
print_tree_stats();
wkd = fltget(fp, &w->cam.eye.x) && fltget(fp, &w->cam.eye.y);
wkd = (wkd && fltget(fp, &w->cam.eye.z) && fltget(fp, &w->cam.yaw));
wkd = (wkd && fltget(fp, &w->cam.pitch) && fltget(fp, &w->cam.roll));
wkd = (wkd && fltget(fp, &w->cam.fov));
if(!wkd) {
fprintf(stderr, "*!Viewpoint\n");
return NULL;
}
w->cam.pitch = to_radians(w->cam.pitch);
w->cam.yaw = to_radians(w->cam.yaw);
w->cam.roll = to_radians(w->cam.roll);
w->cam.fov = to_radians(w->cam.fov);
return w.release();
}
void test_vec3() {
// Vec4
/*
struct vec4 v1 = { 1, 2, 3, 4 };
struct vec4 v2 = { 2, 2, 2, 2 };
assert(vec4_dot(&v1, &v2) == 2);
struct vec4 v3 = vec4_sub(&v1, &v2);
assert(v3.x == -1);
assert(v3.y == 0);
assert(v3.z == 1);
assert(v3.w == 2);
struct vec4 v3 = v1 + v2;
assert(v3.x == 3);
assert(v3.y == 4);
assert(v3.z == 5);
assert(v3.w == 6);
struct vec4 v3 = 2 * v1;
assert(v3.x == 2);
assert(v3.y == 4);
assert(v3.z == 6);
assert(v3.w == 8);
*/
// Vec3
struct vec3 v1 = {1, 2, 3};
struct vec3 v2 = {2, 2, 2};
assert(vec3_dot(&v1, &v2) == 12);
struct vec3 v3 = vec3_sub(&v1, &v2);
assert(v3.x == -1);
assert(v3.y == 0);
assert(v3.z == 1);
struct vec3 v4 = vec3_add(&v1, &v2);
assert(v4.x == 3);
assert(v4.y == 4);
assert(v4.z == 5);
struct vec3 v5 = vec3_scale(&v1, 2);
assert(v5.x == 2);
assert(v5.y == 4);
assert(v5.z == 6);
struct vec3 vx = {1, 0, 0};
struct vec3 vy = {0, 1, 0};
struct vec3 vz = {0, 0, 1};
struct vec3 v6;
v6 = vec3_cross(&vx, &vy);
assert(v6.x == 0 && v6.y == 0 && v6.z == 1);
v6 = vec3_cross(&vz, &vx);
assert(v6.x == 0 && v6.y == 1 && v6.z == 0);
v6 = vec3_cross(&vy, &vz);
assert(v6.x == 1 && v6.y == 0 && v6.z == 0);
/*
// Vec2
local v1 = vec.Vec2(1, 2)
local v2 = vec.Vec2(2, 2)
assert(v1:dot(v2) == 6)
local v3 = v1 - v2
assert(v3.x == -1)
assert(v3.y == 0)
local v3 = v1 + v2
assert(v3.x == 3)
assert(v3.y == 4)
local v3 = 2 * v1
assert(v3.x == 2)
assert(v3.y == 4)
assert(v3:data()[0] == 2)
assert(v3:data()[1] == 4)
assert(ffi.sizeof(v3) == ffi.sizeof('vec_Scalar')*2)
assert(v3 == vec.Vec2(2, 4))
*/
}
void objpose(mat33_t *R, vec3_t *t, int *it, real_t *obj_err, real_t *img_err,
bool calc_img_err, const vec3_t *_P, const vec3_t *Qp, const options_t options, const int n)
{
int i, j;
//vec3_array P(_P.begin(),_P.end());
vec3_t P[n];
memcpy(&*P, _P, n*sizeof(vec3_t));
//const int n = (unsigned int) P.size();
vec3_t pbar;
vec3_array_sum(&pbar, &*P, n);
vec3_div(&pbar, (real_t)(n));
vec3_array_sub(&*P, &pbar, n);
//vec3_array Q(Qp.begin(),Qp.end());
vec3_t Q[n];
memcpy(&*Q, Qp, n*sizeof(vec3_t));
vec3_t ones;
ones.v[0] = 1;
ones.v[1] = 1;
ones.v[2] = 1;
const bool mask_z[3] = {0,0,1};
vec3_array_set(&*Q, &ones, mask_z, n);
//mat33_array F;
//F.resize(n);
mat33_t F[n];
vec3_t V;
for(i=0; i<n; i++)
{
V.v[0] = Q[i].v[0] / Q[i].v[2];
V.v[1] = Q[i].v[1] / Q[i].v[2];
V.v[2] = 1.0;
mat33_t _m;
vec3_mul_vec3trans(&_m, &V, &V);
mat33_div(&_m, vec3trans_mul_vec3(&V,&V));
F[i] = _m;
}
mat33_t tFactor;
mat33_t _m1,_m2,_m3;
mat33_eye(&_m1);
mat33_array_sum(&_m2, &*F, n);
mat33_div(&_m2, (real_t)(n));
mat33_sub_mat2(&_m3, &_m1, &_m2);
mat33_inv(&tFactor, &_m3);
mat33_div(&tFactor, (real_t)(n));
*it = 0;
int initR_approximate = mat33_all_zeros(&options.initR);
mat33_t Ri;
vec3_t ti;
//vec3_array Qi;
//Qi.resize(n);
vec3_t Qi[n];
real_t old_err = 0.0, new_err = 0.0;
// ----------------------------------------------------------------------------------------
if(initR_approximate == 0)
{
mat33_copy(&Ri, &options.initR);
vec3_t _sum;
vec3_t _v1, _v2;
mat33_t _m1,_m2;
vec3_clear(&_sum);
for(j=0; j<n; j++)
{
mat33_eye(&_m1);
mat33_sub_mat2(&_m2, &F[j], &_m1);
vec3_mult_mat(&_v1, &Ri, &P[j]);
vec3_mult_mat(&_v2, &_m2, &_v1);
vec3_add_vec(&_sum, &_v2);
}
vec3_mult_mat(&ti,&tFactor,&_sum);
xform(&*Qi, &*P, &Ri, &ti, n);
old_err = 0;
vec3_t _v;
for(j=0; j<n; j++)
{
mat33_eye(&_m1);
mat33_sub_mat2(&_m2, &F[j], &_m1);
vec3_mult_mat(&_v, &_m2, &Qi[j]);
old_err += vec3_dot(&_v, &_v);
}
// ----------------------------------------------------------------------------------------
}
else
{
abskernel(&Ri, &ti, &*Qi, &old_err, &*P, &*Q, &*F, &tFactor, n);
*it = 1;
}
// ----------------------------------------------------------------------------------------
abskernel(&Ri, &ti, &*Qi, &new_err, &*P, &*Qi, &*F, &tFactor, n);
*it = *it + 1;
while((_abs((old_err-new_err)/old_err) > options.tol) && (new_err > options.epsilon) &&
(options.max_iter == 0 || *it < options.max_iter))
{
old_err = new_err;
abskernel(&Ri, &ti, &*Qi, &new_err, &*P, &*Qi, &*F, &tFactor, n);
*it = *it + 1;
}
mat33_copy(R, &Ri);
vec3_copy(t, &ti);
*obj_err = _sqrt(new_err/(real_t)(n));
if(calc_img_err == 1)
{
//vec3_array Qproj;
//Qproj.resize(n);
vec3_t Qproj[n];
xformproj(&*Qproj, &*P, &Ri, &ti, n);
*img_err = 0;
vec3_t _v;
for(j=0; j<n; j++)
{
vec3_sub_vec2(&_v, &Qproj[j], &Qp[j]);
*img_err += vec3_dot(&_v, &_v);
}
*img_err = _sqrt(*img_err/(real_t)(n));
}
if(t->v[2] < 0)
{
mat33_mult(R, -1.0);
vec3_mult(t, -1.0);
}
vec3_t _ts;
vec3_mult_mat(&_ts, &Ri, &pbar);
vec3_sub_vec(t, &_ts);
}
bool operator() (const sphere& s1, const sphere& s2)
{
vec3 diff = vec3_subtract(s2.center, s1.center);
return vec3_dot(diff, split_plane_normal) > 0;
}
double vec3_length(Vec3 a)
{
return sqrt(vec3_dot(a, a));
}
