void SlideNavmesh::update(const float dt)
{
if (!m_update && !m_step)
return;
m_step = false;
const float maxSpeed = 1.0f;
NavmeshAgent* agent = &m_scene.agents[0];
// Find next corner to steer to.
// Smooth corner finding does a little bit of magic to calculate spline
// like curve (or first tangent) based on current position and movement direction
// next couple of corners.
float corner[2],dir[2];
int last = 1;
vsub(dir, agent->pos, agent->oldpos); // This delta handles wall-hugging better than using current velocity.
vnorm(dir);
vcpy(corner, agent->pos);
if (m_moveMode == AGENTMOVE_SMOOTH || m_moveMode == AGENTMOVE_DRUNK)
last = agentFindNextCornerSmooth(agent, dir, m_scene.nav, corner);
else
last = agentFindNextCorner(agent, m_scene.nav, corner);
if (last && vdist(agent->pos, corner) < 0.02f)
{
// Reached goal
vcpy(agent->oldpos, agent->pos);
vset(agent->dvel, 0,0);
vcpy(agent->vel, agent->dvel);
return;
}
vsub(agent->dvel, corner, agent->pos);
// Apply style
if (m_moveMode == AGENTMOVE_DRUNK)
{
agent->t += dt*4;
float amp = cosf(agent->t)*0.25f;
float nx = -agent->dvel[1];
float ny = agent->dvel[0];
agent->dvel[0] += nx * amp;
agent->dvel[1] += ny * amp;
}
// Limit desired velocity to max speed.
const float distToTarget = vdist(agent->pos,agent->target);
const float clampedSpeed = maxSpeed * min(1.0f, distToTarget/agent->rad);
vsetlen(agent->dvel, clampedSpeed);
vcpy(agent->vel, agent->dvel);
// Move agent
vscale(agent->delta, agent->vel, dt);
float npos[2];
vadd(npos, agent->pos, agent->delta);
agentMoveAndAdjustCorridor(&m_scene.agents[0], npos, m_scene.nav);
}
bool SlideNavmesh::mouseDown(const float x, const float y)
{
if (!m_expanded)
{
if (hitCorner(x,y))
{
startDrag(1, x,y, m_pos);
return true;
}
}
else
{
int bidx = hitButtons(x-m_pos[0],y-m_pos[1]);
if (bidx != -1)
{
return true;
}
else if (hitCorner(x,y))
{
startDrag(1, x,y, m_pos);
return true;
}
else if (hitArea(x,y))
{
const float lx = x - (m_pos[0]+PADDING_SIZE);
const float ly = y - (m_pos[1]+PADDING_SIZE);
float pos[2] = {lx,ly};
float nearest[2] = {lx,ly};
if (m_scene.nav)
navmeshFindNearestTri(m_scene.nav, pos, nearest);
if (SDL_GetModState() & KMOD_SHIFT)
{
agentMoveAndAdjustCorridor(&m_scene.agents[0], nearest, m_scene.nav);
vcpy(m_scene.agents[0].oldpos, m_scene.agents[0].pos);
vset(m_scene.agents[0].corner, FLT_MAX,FLT_MAX);
}
else
{
vcpy(m_scene.agents[0].target, nearest);
vcpy(m_scene.agents[0].oldpos, m_scene.agents[0].pos);
agentFindPath(&m_scene.agents[0], m_scene.nav);
vset(m_scene.agents[0].corner, FLT_MAX,FLT_MAX);
}
return true;
}
}
return false;
}
/*
* Find a intersection point for each beam's corner ray onto the triangle
* plane.
* Points found may not lie within the triangle.
*/
int
find_isect_pos_onto_the_triangle_plane(
ri_vector_t *points, /* [out] */
ri_beam_t *beam,
ri_triangle_t *triangle)
{
int i;
ri_vector_t v0, v1, v2;
ri_vector_t e1, e2;
ri_vector_t p, s, q;
ri_float_t a, inva;
ri_float_t t;
double eps = 1.0e-14;
vcpy( v0, triangle->v[0] );
vcpy( v1, triangle->v[1] );
vcpy( v2, triangle->v[2] );
vsub( e1, v1, v0 );
vsub( e2, v2, v0 );
vsub( s, beam->org, v0 );
for (i = 0; i < 4; i++) {
vcross( p, beam->dir[i], e2 );
a = vdot( e1, p );
if (fabs(a) > eps) {
inva = 1.0 / a;
} else {
inva = 1.0;
}
vcross( q, s, e1 );
t = vdot( e2, q ) * inva;
points[i][0] = beam->org[0] + t * beam->dir[i][0];
points[i][1] = beam->org[1] + t * beam->dir[i][1];
points[i][2] = beam->org[2] + t * beam->dir[i][2];
}
return 0;
}
/*
* Rasterize beam into 2D raster plane.
*/
void
ri_rasterize_beam(
ri_raster_plane_t *plane,
ri_beam_t *beam,
ri_triangle_t *triangle)
{
int i;
int n;
ri_vector_t points[4]; /* Points onto the plane defined by
the triangle. */
ri_triangle_t triangles[2]; /* up to 2 triangles. */
//
// Find hit points onto triangle plane.
//
find_isect_pos_onto_the_triangle_plane(
points,
beam,
triangle);
if (beam->is_tetrahedron) {
// Raster single triangle.
vcpy(triangles[0].v[0], points[0]);
vcpy(triangles[0].v[1], points[1]);
vcpy(triangles[0].v[2], points[2]);
n = 1;
} else {
// Raster 2 triangles.
vcpy(triangles[0].v[0], points[0]);
vcpy(triangles[0].v[1], points[1]);
vcpy(triangles[0].v[2], points[2]);
vcpy(triangles[1].v[0], points[0]);
vcpy(triangles[1].v[1], points[2]);
vcpy(triangles[1].v[2], points[3]);
n = 2;
}
for (i = 0; i < n; i++) {
ri_rasterize_triangle(plane, &triangles[i]);
}
}
static void trace_whitted(
ri_render_t *render,
ri_intersection_state_t *isect,
ri_transport_info_t *result,
int depth)
{
double eps = 1.0e-7;
int hit;
ri_intersection_state_t state;
ri_ray_t Rr;
vec Rd;
ri_ray_t Tr;
vec Td;
ri_float_t eta = 1.33;
if (depth > MAX_TRACE_DEPTH) {
return;
}
//ri_reflect(Rd, isect->I, isect->Ns);
ri_refract(Rd, isect->I, isect->Ns, 1.33);
vcpy(Rr.dir, Rd);
Rr.org[0] = isect->P[0] + eps * Rd[0];
Rr.org[1] = isect->P[1] + eps * Rd[1];
Rr.org[2] = isect->P[2] + eps * Rd[2];
hit = ri_raytrace(render, &Rr, &state);
if (hit) {
//vzero(result->radiance);
trace_whitted(render, &state, result, depth + 1);
} else {
/*
* If the scene has envmap, add contribution from the envmap.
*/
if (render->scene->envmap_light) {
ri_texture_ibl_fetch(result->radiance,
render->scene->envmap_light->texture,
Rd);
} else {
vzero(result->radiance);
}
}
}
void
sample_distant_light(
ri_vector_t Lo, /* [out] */
ri_bvh_t *bvh,
const ri_intersection_state_t *isect,
ri_vector_t Ldir, /* normalized */
ri_vector_t Lcol,
int debug)
{
(void)debug;
int hit;
ri_ray_t ray;
ri_intersection_state_t state;
ri_float_t dot;
vcpy(ray.org, isect->P);
ray.org[0] += isect->Ns[0] * 0.00001;
ray.org[1] += isect->Ns[1] * 0.00001;
ray.org[2] += isect->Ns[2] * 0.00001;
vcpy(ray.dir, Ldir);
hit = ri_bvh_intersect( (void *)bvh, &ray, &state, NULL );
if (hit) {
/* There's obscrances between the shading point and the light */
vzero(Lo);
} else {
/* Lo = L cosTheta */
dot = vdot(Ldir, isect->Ns);
if (dot < 0.0) dot = 0.0;
Lo[0] = Lcol[0] * dot;
Lo[1] = Lcol[1] * dot;
Lo[2] = Lcol[2] * dot;
}
}
/*
* Add the vertex to output.
*/
void
output(
point2d_t *list_out, /* [out] */
int *len_inout, /* [inout] */
point2d_t p)
{
int idx;
idx = (*len_inout);
vcpy( list_out[idx], p );
(*len_inout)++;
}
static int
contribution_from_sunlight(
vec Lo,
const ri_ray_t *inray,
const ri_intersection_state_t *isect)
{
ri_list_t *light_list;
ri_light_t *light;
int hit;
ri_ray_t ray;
ri_intersection_state_t state;
double eps = 1.0e-5;
for (light_list = ri_list_first(ri_render_get()->scene->light_list);
light_list != NULL;
light_list = ri_list_next(light_list)) {
light = (ri_light_t *)light_list->data;
if (light->type != LIGHTTYPE_SUNLIGHT) continue;
vcpy(ray.org, isect->P);
ray.org[0] += isect->Ns[0] * eps;
ray.org[1] += isect->Ns[1] * eps;
ray.org[2] += isect->Ns[2] * eps;
ray.dir[0] = light->direction[0];
ray.dir[1] = light->direction[1];
ray.dir[2] = light->direction[2];
ray.thread_num = inray->thread_num;
hit = ri_raytrace(ri_render_get(), &ray, &state);
if (!hit) {
Lo[0] += light->col[0];
Lo[1] += light->col[1];
Lo[2] += light->col[2];
}
}
}
void sph_model::zoom(double *w, const double *v)
{
double d = vdot(v, zoomv);
if (-1 < d && d < 1)
{
double b = scale(zoomk, acos(d) / M_PI) * M_PI;
double y[3];
double x[3];
vcrs(y, v, zoomv);
vnormalize(y, y);
vcrs(x, zoomv, y);
vnormalize(x, x);
vmul(w, zoomv, cos(b));
vmad(w, w, x, sin(b));
}
else vcpy(w, v);
}
void KX_ObstacleSimulation::UpdateObstacles()
{
for (size_t i=0; i<m_obstacles.size(); i++)
{
if (m_obstacles[i]->m_type==KX_OBSTACLE_NAV_MESH || m_obstacles[i]->m_shape==KX_OBSTACLE_SEGMENT)
continue;
KX_Obstacle* obs = m_obstacles[i];
obs->m_pos = obs->m_gameObj->NodeGetWorldPosition();
obs->vel[0] = obs->m_gameObj->GetLinearVelocity().x();
obs->vel[1] = obs->m_gameObj->GetLinearVelocity().y();
// Update velocity history and calculate perceived (average) velocity.
vcpy(&obs->hvel[obs->hhead*2], obs->vel);
obs->hhead = (obs->hhead+1) % VEL_HIST_SIZE;
vset(obs->pvel,0,0);
for (int j = 0; j < VEL_HIST_SIZE; ++j)
vadd(obs->pvel, obs->pvel, &obs->hvel[j*2]);
vscale(obs->pvel, obs->pvel, 1.0f/VEL_HIST_SIZE);
}
}
void view_move(view *V, const double *v)
{
assert(V);
vcpy(V->v, v);
}
int
ri_raster_plane_setup(
ri_raster_plane_t *plane_inout, /* [inout] */
int width,
int height,
ri_vector_t frame[3],
ri_vector_t corner,
ri_vector_t org,
ri_float_t fov)
{
size_t sz;
sz = width * height;
plane_inout->width = width;
plane_inout->height = height;
if (plane_inout->t ) ri_mem_free(plane_inout->t);
if (plane_inout->u ) ri_mem_free(plane_inout->u);
if (plane_inout->v ) ri_mem_free(plane_inout->v);
if (plane_inout->geom ) ri_mem_free(plane_inout->geom);
if (plane_inout->index) ri_mem_free(plane_inout->index);
plane_inout->t = (ri_float_t *)ri_mem_alloc(sizeof(ri_float_t ) * sz);
plane_inout->u = (ri_float_t *)ri_mem_alloc(sizeof(ri_float_t ) * sz);
plane_inout->v = (ri_float_t *)ri_mem_alloc(sizeof(ri_float_t ) * sz);
plane_inout->geom = (ri_geom_t **)ri_mem_alloc(sizeof(ri_geom_t *) * sz);
plane_inout->index = (uint32_t *)ri_mem_alloc(sizeof(uint32_t ) * sz);
memset(plane_inout->t , 0, sizeof(ri_float_t ) * sz);
memset(plane_inout->u , 0, sizeof(ri_float_t ) * sz);
memset(plane_inout->v , 0, sizeof(ri_float_t ) * sz);
memset(plane_inout->geom , 0, sizeof(ri_geom_t *) * sz);
memset(plane_inout->index, 0, sizeof(uint32_t ) * sz);
memcpy(plane_inout->frame, frame, sizeof(ri_vector_t) * 3);
vcpy( plane_inout->corner, corner );
vcpy( plane_inout->org , org );
plane_inout->fov = fov;
/*
* Project lower-left point onto NDC coord.
*/
/*
* p = M F v
* p' = p / p.z - [0, 1)^2
*
* | 1 |
* M = | ---------- 0 0 0 |
* | tan(fov/2) |
* | |
* | 1 |
* | 0 ---------- 0 0 |
* | tan(fov/2) |
* | |
* | 0 0 1 0 |
* | |
* | 0 0 -1 0 |
*
* F = | du_x du_y du_z 0 |
* | dv_x dv_y dv_z 0 |
* | -dw_x -dw_y -dw_z 0 |
* | 0 0 0 0 |
*
* v = | x |
* | y |
* | z |
* | w |
*/
{
vec vo;
vec w;
vec p;
ri_float_t fov_rad = plane_inout->fov * M_PI / 180.0;
/* vo = v */
vcpy( vo, plane_inout->corner );
/* w = F v */
w[0] = plane_inout->frame[0][0] * vo[0]
+ plane_inout->frame[0][1] * vo[1]
+ plane_inout->frame[0][2] * vo[2];
w[1] = plane_inout->frame[1][0] * vo[0]
+ plane_inout->frame[1][1] * vo[1]
+ plane_inout->frame[1][2] * vo[2];
w[2] = -plane_inout->frame[2][0] * vo[0]
- plane_inout->frame[2][1] * vo[1]
- plane_inout->frame[2][2] * vo[2];
/* p = M F v */
p[0] = (1.0 / tan(0.5 * fov_rad)) * w[0];
p[1] = (1.0 / tan(0.5 * fov_rad)) * w[1];
p[2] = w[2];
p[0] /= -w[2]; /* w = -z */
p[1] /= -w[2];
printf("[raster] lowerleft = %f, %f\n", p[0], p[1]);
plane_inout->offset[0] = p[0];
plane_inout->offset[1] = p[1];
}
return 0;
}
/*
* Function: ri_bem_set
*
* Setups a beam structure.
*
*
* Parameters:
*
* beam - Pointer to the beam to be set up.
* org - Origin of beam.
* dir[4] - Corner rays of beam.
*
*
* Returns:
*
* 0 if OK, otherwise if err
*/
int
ri_beam_set(
ri_beam_t *beam, /* [inout] */
ri_vector_t org,
ri_vector_t dir[4])
{
int i, j;
int mask;
int dominant_axis;
int zeros;
ri_float_t maxval;
beam->d = 1024.0;
beam->t_max = RI_INFINITY;
/*
* Check if beam's directions lie in same quadrant.
*/
for (i = 0; i < 3; i++) {
zeros = 0;
mask = 0;
for (j = 0; j < 4; j++) {
if (fabs(dir[j][i]) < RI_EPS) {
zeros++;
} else {
mask += (dir[j][i] < 0.0) ? 1 : -1;
}
}
if ( (mask != -(4 - zeros)) && (mask != (4 - zeros)) ) {
/* FIXME:
* split beam so that subdivided beam has same sign.
*/
fprintf(stderr, "TODO: Beam's dir does not have same sign.\n");
for (j = 0; j < 4; j++) {
fprintf(stderr, " dir[%d] = %f, %f, %f\n", j,
dir[j][0], dir[j][1], dir[j][2]);
}
return -1;
}
}
vcpy( beam->org, org );
/*
* Find dominant plane. Use dir[0]
*/
maxval = fabs(dir[0][0]);
dominant_axis = 0;
if ( (maxval < fabs(dir[0][1])) ) {
maxval = fabs(dir[0][0]);
dominant_axis = 1;
}
if ( (maxval < fabs(dir[0][2])) ) {
maxval = fabs(dir[0][2]);
dominant_axis = 2;
}
beam->dominant_axis = dominant_axis;
/*
* Precompute sign of direction.
* We know all 4 directions has same sign, thus dir[0] is used to
* get sign of direction for each axis.
*/
beam->dirsign[0] = (dir[0][0] < 0.0) ? 1 : 0;
beam->dirsign[1] = (dir[0][1] < 0.0) ? 1 : 0;
beam->dirsign[2] = (dir[0][2] < 0.0) ? 1 : 0;
/*
* Project beam dir onto axis-alied plane.
*/
{
ri_vector_t normals[3] = {
{ 1.0, 0.0, 0.0 }, { 0.0, 1.0, 0.0 }, { 0.0, 0.0, 1.0 } };
ri_vector_t normal;
ri_float_t t;
ri_float_t k;
vcpy( normal, normals[beam->dominant_axis] );
if (beam->dirsign[beam->dominant_axis]) {
vneg( normal );
}
for (i = 0; i < 4; i++) {
t = vdot( dir[i], normal );
if (fabs(t) > RI_EPS) {
k = beam->d / t;
} else {
k = 1.0;
}
beam->dir[i][0] = k * dir[i][0];
beam->dir[i][1] = k * dir[i][1];
beam->dir[i][2] = k * dir[i][2];
}
}
/*
* Precompute inverse of direction
*/
for (i = 0; i < 4; i++) {
beam->invdir[i][0] = safeinv( beam->dir[i][0], RI_EPS, RI_FLT_MAX );
beam->invdir[i][1] = safeinv( beam->dir[i][1], RI_EPS, RI_FLT_MAX );
beam->invdir[i][2] = safeinv( beam->dir[i][2], RI_EPS, RI_FLT_MAX );
}
/*
* Precompute normal plane of beam frustum
*/
vcross( beam->normal[0], beam->dir[1], beam->dir[0] );
vcross( beam->normal[1], beam->dir[2], beam->dir[1] );
vcross( beam->normal[2], beam->dir[3], beam->dir[2] );
vcross( beam->normal[3], beam->dir[0], beam->dir[3] );
beam->is_tetrahedron = 0;
return 0; /* OK */
}
/*
* Derived version of ambient occlusion: gather sunsky color instead of
* just computing occlusion.
*/
static int
gather_sunsky(
ri_vector_t Lo, /* [out] */
const ri_ray_t *inray,
const ri_intersection_state_t *isect,
uint32_t ntheta_samples,
uint32_t nphi_samples)
{
int hit;
uint32_t i, j, k;
int thread_id = 0;
double z0, z1;
double cos_theta, phi;
double eps = 1.0e-5;
vec dir;
vec basis[3];
vec col;
float sunskycol[3];
float v[3];
ri_ray_t ray;
ri_intersection_state_t state;
ri_ortho_basis(basis, isect->Ns);
vcpy(ray.org, isect->P);
vzero(col);
/*
* Slightly move the shading point towards the surface normal.
* FIXME: Choose eps relative to scene scale, not as an absolute value.
*/
ray.org[0] += isect->Ns[0] * eps;
ray.org[1] += isect->Ns[1] * eps;
ray.org[2] += isect->Ns[2] * eps;
thread_id = inray->thread_num;
assert(thread_id >= 0);
assert(thread_id < 16);
ray.thread_num = thread_id;
for (j = 0; j < nphi_samples; j++) {
for (i = 0; i < ntheta_samples; i++) {
/*
* 1. Choose random ray direction over the hemisphere.
*/
/* Simple stratified sampling */
z0 = (i + randomMT2(thread_id)) / (double)ntheta_samples;
z1 = (j + randomMT2(thread_id)) / (double)nphi_samples;
/* Do importance sampling. the probability function is,
*
* p(x) ~ cos(theta) / PI (against with differential solid angle)
*
* -> theta = acos(sqrt(z_0))
* phi = 2 PI z_1
*
*/
cos_theta = sqrt(z0);
phi = 2.0 * M_PI * z1;
dir[0] = cos(phi) * cos_theta;
dir[1] = sin(phi) * cos_theta;
dir[2] = sqrt(1.0 - cos_theta * cos_theta);
/* 'dir' is defined in local coord.
* Convert it into global coord.
*/
for (k = 0; k < 3; k++) {
ray.dir[k] = dir[0]*basis[0][k]
+ dir[1]*basis[1][k]
+ dir[2]*basis[2][k];
}
/*
* 2. Do raytracing to check visibility.
*/
hit = ri_raytrace(ri_render_get(), &ray, &state);
if (!hit) {
v[0] = ray.dir[0];
v[1] = ray.dir[1];
v[2] = ray.dir[2];
ri_sunsky_get_sky_rgb(
sunskycol,
ri_render_get()->scene->sunsky_light->sunsky,
v);
col[0] += sunskycol[0];
col[1] += sunskycol[1];
col[2] += sunskycol[2];
}
}
}
/*
* Add contribution from sun.
*/
contribution_from_sunlight(col, inray, isect);
double nsamples = ntheta_samples * nphi_samples;
double m =(1.0 / M_PI);
Lo[0] = m * col[0] / nsamples;
Lo[1] = m * col[1] / nsamples;
Lo[2] = m * col[2] / nsamples;
return 0; /* OK */
}
static int
calculate_occlusion(
ri_vector_t Lo, /* [out] */
const ri_ray_t *inray,
const ri_intersection_state_t *isect,
uint32_t ntheta_samples,
uint32_t nphi_samples)
{
int hit;
uint32_t i, j, k;
int thread_id = 0;
double z0, z1;
double cos_theta, phi;
double eps = 1.0e-6;
double occlusion = 0.0;
vec dir;
vec basis[3];
ri_ray_t ray;
ri_intersection_state_t state;
ri_ortho_basis(basis, isect->Ns);
vcpy(ray.org, isect->P);
/*
* Slightly move the shading point towards the surface normal.
* FIXME: Choose eps relative to scene scale, not as an absolute value.
*/
ray.org[0] += isect->Ns[0] * eps;
ray.org[1] += isect->Ns[1] * eps;
ray.org[2] += isect->Ns[2] * eps;
thread_id = inray->thread_num;
assert(thread_id >= 0);
assert(thread_id < 16);
ray.thread_num = thread_id;
for (j = 0; j < nphi_samples; j++) {
for (i = 0; i < ntheta_samples; i++) {
/*
* 1. Choose random ray direction over the hemisphere.
*/
/* Simple stratified sampling */
z0 = (i + randomMT2(thread_id)) / (double)ntheta_samples;
z1 = (j + randomMT2(thread_id)) / (double)nphi_samples;
/* Do importance sampling. the probability function is,
*
* p(x) ~ cos(theta) / PI (against with differential solid angle)
*
* -> theta = acos(sqrt(z_0))
* phi = 2 PI z_1
*
*/
cos_theta = sqrt(z0);
phi = 2.0 * M_PI * z1;
dir[0] = cos(phi) * cos_theta;
dir[1] = sin(phi) * cos_theta;
dir[2] = sqrt(1.0 - cos_theta * cos_theta);
/* 'dir' is defined in local coord.
* Convert it into global coord.
*/
for (k = 0; k < 3; k++) {
ray.dir[k] = dir[0]*basis[0][k]
+ dir[1]*basis[1][k]
+ dir[2]*basis[2][k];
}
/*
* 2. Do raytracing to check visibility.
*/
hit = ri_raytrace(ri_render_get(), &ray, &state);
if (hit) {
/* There's an occluder. */
occlusion += 1.0;
}
}
}
/*
* Turn occlusion value into color(radiance)
*
* Lo = m * (N - occlusion) / N
*
* where N = ntheta * nphi
* m = pi if phisically accurate value is required.
* 1.0 otherwise
*/
double nsamples = ntheta_samples * nphi_samples;
double m = 1.0; // (1.0 / M_PI)
Lo[0] = m * (nsamples - occlusion) / nsamples;
Lo[1] = m * (nsamples - occlusion) / nsamples;
Lo[2] = m * (nsamples - occlusion) / nsamples;
return 0; /* OK */
}
/*
* Function: trace_path
*
* Samples light transport path in recursive manner. In curent
* implementation, <trace_path> acts as distribution ray tracing.
*
* Parameters:
*
* *render - The global renderer data.
* *ray - The ray to be traced.
* *resut - Light transport result(including radiance).
*
* Returns:
*
* None.
*/
static void
trace_path(ri_render_t *render, ri_ray_t *ray, ri_transport_info_t *result)
{
int max_nbound_specular = 10;
if (result->nbound_specular > max_nbound_specular) {
/* Too much reflection, terminate. */
return;
}
ri_light_t *light = NULL;
int hit;
/* hack */
vec white;
vec black;
ri_vector_set1(white, 1.0);
ri_vector_set1(black, 0.0);
hit = ri_raytrace(render, ray, &(result->state));
if (hit) {
if (result->state.geom->light) {
light = result->state.geom->light;
/* Hit light geometry. */
vcpy(result->radiance, light->col);
return;
}
vcpy(result->radiance, white);
} else {
vcpy(result->radiance, black);
}
return;
#if 0 // TODO
int hit, lighthit;
int hasfresnel;
ri_vector_t col;
ri_vector_t transmit_ray;
ri_vector_t reflect_ray;
ri_vector_t offset;
ri_vector_t raydir;
ri_vector_t rayorg;
ri_vector_t refrad, trasrad;
ri_vector_t normal;
ri_light_t *light;
ri_vector_t rad;
ri_material_t *material;
ri_ray_t lightray;
ri_transport_info_t ref_result; /* for reflection */
double r, d, s, t;
float fresnel_factor = 1.0f;
float kr, kt;
float eta = 1.0f / 1.4f;
float etaval;
if (result->nbound_specular > 8) {
//printf("too reflection\n");
return;
}
light = get_light(render);
ri_vector_copy(&raydir, ray->dir);
result->state.inside = 0;
hit = ri_raytrace(render, ray, &(result->state));
if (hit) {
if (light->geom) {
/* Check if a ray also hits light geometry and
* that is closer than scene geometry or not.
*/
ri_vector_copy(&lightray.org, ray->org);
ri_vector_copy(&lightray.dir, ray->dir);
lighthit = ri_raytrace_geom(
light->geom,
&lightray,
&(result->state));
if (lighthit && (lightray.isectt < ray->isectt) ) {
// light is "seen"
ri_vector_copy(&result->radiance,
light->col);
result->hit = 1;
return;
}
}
r = randomMT();
material = result->state.geom->material;
if (!material) {
d = 1.0;
s = 0.0;
t = 0.0;
} else {
d = ri_vector_ave(&material->kd);
s = ri_vector_ave(&material->ks);
t = ri_vector_ave(&material->kt);
}
if (s > 0.0) {
/* specular reflection */
if (result->state.geom->material &&
result->state.geom->material->fresnel) {
/* Fresnel reflection */
ri_fresnel(&ray->dir, &transmit_ray,
&kr, &kt,
&ray->dir, &result->state.normal,
eta);
fresnel_factor = kr;
} else {
ri_reflect(&(ray->dir),
&ray->dir,
&result->state.normal);
fresnel_factor = 1.0f;
}
ri_vector_copy(&(ray->org), result->state.P);
ri_vector_copy(&col, result->state.color);
result->nbound_specular++;
/* push radiance */
ri_vector_copy(&rad, result->radiance);
ri_vector_zero(&(result->radiance));
trace_path(render, ray, result);
/* pop radiance */
ri_vector_mul(&(result->radiance),
&result->radiance, &material->ks);
ri_vector_mul(&(result->radiance),
&result->radiance, &col);
ri_vector_scale(&(result->radiance),
fresnel_factor);
ri_vector_add(&(result->radiance),
&result->radiance,
&rad);
}
if (d > 0.0) {
/* diffuse reflection */
result->nbound_diffuse++;
ri_shade(&rad, &ray->dir, ray, &(result->state));
ri_vector_mul(&rad,
&rad, &material->kd);
ri_vector_add(&(result->radiance), &result->radiance,
&rad);
}
if (t > 0.0) {
/* specular refraction */
if (result->state.geom->material &&
result->state.geom->material->fresnel) {
hasfresnel = 1;
} else {
hasfresnel = 0;
}
if (hasfresnel) {
/* Fresnel effect */
ri_vector_copy(&normal,
result->state.normal);
if (result->state.inside) {
printf("inside val = %d\n", result->state.inside);
printf("inside\n");
/* ray hits inside surface */
//ri_vector_neg(&normal);
etaval = 1.0 / eta;
} else {
etaval = eta;
}
ri_fresnel(&reflect_ray, &transmit_ray,
&kr, &kt,
&ray->dir, &normal,
etaval);
} else {
ri_refract(&(ray->dir),
&ray->dir,
&result->state.normal,
eta);
kr = 0.0; kt = 1.0;
}
/* slightly moves the ray towards outgoing direction */
ri_vector_copy(&rayorg, result->state.P);
/* ray.org = ray.org + 0.001 * ray.dir */
ri_vector_copy(&offset, &transmit_ray);
ri_vector_scale(&offset, 0.001);
ri_vector_add(&(ray->org), &rayorg, &offset);
/* ray.dir = refract direction */
ri_vector_copy(&(ray->dir), &transmit_ray);
ri_vector_copy(&col, &result->state.color);
result->nbound_specular++;
ray->prev_hit = 'S';
/* push radiance */
ri_vector_copy(&rad, &result->radiance);
ri_vector_zero(&(result->radiance));
trace_path(render, ray, result);
/* pop radiance */
ri_vector_mul(&(result->radiance),
&result->radiance, &material->kt);
ri_vector_mul(&(result->radiance),
&result->radiance, &col);
ri_vector_scale(&(result->radiance), kt);
if (hasfresnel) {
/* add reflection color */
/* ray.org = ray.org + 0.001 * ray.dir */
ri_vector_copy(&offset, &reflect_ray);
ri_vector_scale(&offset, 0.001);
ri_vector_add(&(ray->org), &rayorg, &offset);
ri_vector_copy(&(ray->dir), &reflect_ray);
ri_vector_copy(&col, &result->state.color);
ray->prev_hit = 'S';
ri_vector_zero(&ref_result.radiance);
ref_result.nbound_specular = result->nbound_specular;
ref_result.nbound_diffuse = result->nbound_diffuse;
ref_result.state.inside = 0;
trace_path(render, ray, &ref_result);
/* pop radiance */
ri_vector_mul(&(ref_result.radiance),
&ref_result.radiance, &col);
ri_vector_scale(&(ref_result.radiance), kr);
ri_vector_add(&(result->radiance),
&result->radiance, &ref_result.radiance);
}
ri_vector_add(&(result->radiance),
&result->radiance,
&rad);
}
//} else if (result->nbound_specular + result->nbound_diffuse < 2) {
} else {
/* check if hit light geometry */
ray->isectt = 0.0f;
if (light->type == LIGHTTYPE_IBL ||
light->type == LIGHTTYPE_SUNSKY) {
ri_texture_ibl_fetch(&(result->radiance),
light->texture,
&ray->dir);
result->hit = 1;
return;
} else if (ri_render_get()->background_map) {
ri_texture_ibl_fetch(&(result->radiance),
ri_render_get()->background_map,
&ray->dir);
result->hit = 1;
return;
} else {
if (light->geom) {
/* area light. */
lighthit = ri_raytrace_geom(
light->geom,
ray,
&(result->state));
if (lighthit) {
// light is "seen"
result->radiance.e[0] = 1.0;
result->radiance.e[1] = 1.0;
result->radiance.e[2] = 1.0;
result->hit = 1;
return;
}
} else if (light->type == LIGHTTYPE_DOME) {
//ri_vector_copy(&(result->radiance),
// &(light->col));
//ri_vector_scale(&(result->radiance),
// (float)light->intensity);
}
}
}
#endif
return;
}
void KX_ObstacleSimulationTOI_cells::sampleRVO(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj,
const float maxDeltaAngle)
{
vset(activeObst->nvel, 0.f, 0.f);
float vmax = vlen(activeObst->dvel);
float* spos = new float[2*m_maxSamples];
int nspos = 0;
if (!m_adaptive)
{
const float cvx = activeObst->dvel[0]*m_bias;
const float cvy = activeObst->dvel[1]*m_bias;
float vmax = vlen(activeObst->dvel);
const float vrange = vmax*(1-m_bias);
const float cs = 1.0f / (float)m_sampleRadius*vrange;
for (int y = -m_sampleRadius; y <= m_sampleRadius; ++y)
{
for (int x = -m_sampleRadius; x <= m_sampleRadius; ++x)
{
if (nspos < m_maxSamples)
{
const float vx = cvx + (float)(x+0.5f)*cs;
const float vy = cvy + (float)(y+0.5f)*cs;
if (vx*vx+vy*vy > sqr(vmax+cs/2)) continue;
spos[nspos*2+0] = vx;
spos[nspos*2+1] = vy;
nspos++;
}
}
}
processSamples(activeObst, activeNavMeshObj, m_obstacles, m_levelHeight, vmax, spos, cs/2,
nspos, activeObst->nvel, m_maxToi, m_velWeight, m_curVelWeight, m_collisionWeight, m_toiWeight);
}
else
{
int rad;
float res[2];
float cs;
// First sample location.
rad = 4;
res[0] = activeObst->dvel[0]*m_bias;
res[1] = activeObst->dvel[1]*m_bias;
cs = vmax*(2-m_bias*2) / (float)(rad-1);
for (int k = 0; k < 5; ++k)
{
const float half = (rad-1)*cs*0.5f;
nspos = 0;
for (int y = 0; y < rad; ++y)
{
for (int x = 0; x < rad; ++x)
{
const float vx = res[0] + x*cs - half;
const float vy = res[1] + y*cs - half;
if (vx*vx+vy*vy > sqr(vmax+cs/2)) continue;
spos[nspos*2+0] = vx;
spos[nspos*2+1] = vy;
nspos++;
}
}
processSamples(activeObst, activeNavMeshObj, m_obstacles, m_levelHeight, vmax, spos, cs/2,
nspos, res, m_maxToi, m_velWeight, m_curVelWeight, m_collisionWeight, m_toiWeight);
cs *= 0.5f;
}
vcpy(activeObst->nvel, res);
}
}
static void processSamples(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj,
KX_Obstacles& obstacles, float levelHeight, const float vmax,
const float* spos, const float cs, const int nspos, float* res,
float maxToi, float velWeight, float curVelWeight, float sideWeight,
float toiWeight)
{
vset(res, 0,0);
const float ivmax = 1.0f / vmax;
float adir[2], adist;
vcpy(adir, activeObst->pvel);
if (vlen(adir) > 0.01f)
vnorm(adir);
else
vset(adir,0,0);
float activeObstPos[2];
vset(activeObstPos, activeObst->m_pos.x(), activeObst->m_pos.y());
adist = vdot(adir, activeObstPos);
float minPenalty = FLT_MAX;
for (int n = 0; n < nspos; ++n)
{
float vcand[2];
vcpy(vcand, &spos[n*2]);
// Find min time of impact and exit amongst all obstacles.
float tmin = maxToi;
float side = 0;
int nside = 0;
for (int i = 0; i < obstacles.size(); ++i)
{
KX_Obstacle* ob = obstacles[i];
bool res = filterObstacle(activeObst, activeNavMeshObj, ob, levelHeight);
if (!res)
continue;
float htmin, htmax;
if (ob->m_shape==KX_OBSTACLE_CIRCLE)
{
float vab[2];
// Moving, use RVO
vscale(vab, vcand, 2);
vsub(vab, vab, activeObst->vel);
vsub(vab, vab, ob->vel);
// Side
// NOTE: dp, and dv are constant over the whole calculation,
// they can be precomputed per object.
const float* pa = activeObstPos;
float pb[2];
vset(pb, ob->m_pos.x(), ob->m_pos.y());
const float orig[2] = {0,0};
float dp[2],dv[2],np[2];
vsub(dp,pb,pa);
vnorm(dp);
vsub(dv,ob->dvel, activeObst->dvel);
const float a = triarea(orig, dp,dv);
if (a < 0.01f)
{
np[0] = -dp[1];
np[1] = dp[0];
}
else
{
np[0] = dp[1];
np[1] = -dp[0];
}
side += clamp(min(vdot(dp,vab)*2,vdot(np,vab)*2), 0.0f, 1.0f);
nside++;
if (!sweepCircleCircle(activeObst->m_pos, activeObst->m_rad, vab, ob->m_pos, ob->m_rad,
htmin, htmax))
continue;
// Handle overlapping obstacles.
if (htmin < 0.0f && htmax > 0.0f)
{
// Avoid more when overlapped.
htmin = -htmin * 0.5f;
}
}
else if (ob->m_shape == KX_OBSTACLE_SEGMENT)
{
MT_Point3 p1 = ob->m_pos;
MT_Point3 p2 = ob->m_pos2;
//apply world transform
if (ob->m_type == KX_OBSTACLE_NAV_MESH)
{
KX_NavMeshObject* navmeshobj = static_cast<KX_NavMeshObject*>(ob->m_gameObj);
p1 = navmeshobj->TransformToWorldCoords(p1);
p2 = navmeshobj->TransformToWorldCoords(p2);
}
float p[2], q[2];
vset(p, p1.x(), p1.y());
vset(q, p2.x(), p2.y());
// NOTE: the segments are assumed to come from a navmesh which is shrunken by
// the agent radius, hence the use of really small radius.
// This can be handle more efficiently by using seg-seg test instead.
// If the whole segment is to be treated as obstacle, use agent->rad instead of 0.01f!
const float r = 0.01f; // agent->rad
if (distPtSegSqr(activeObstPos, p, q) < sqr(r+ob->m_rad))
{
float sdir[2], snorm[2];
vsub(sdir, q, p);
snorm[0] = sdir[1];
snorm[1] = -sdir[0];
// If the velocity is pointing towards the segment, no collision.
if (vdot(snorm, vcand) < 0.0f)
continue;
// Else immediate collision.
htmin = 0.0f;
htmax = 10.0f;
}
else
{
if (!sweepCircleSegment(activeObstPos, r, vcand, p, q, ob->m_rad, htmin, htmax))
continue;
}
// Avoid less when facing walls.
htmin *= 2.0f;
}
if (htmin >= 0.0f)
{
// The closest obstacle is somewhere ahead of us, keep track of nearest obstacle.
if (htmin < tmin)
tmin = htmin;
}
}
// Normalize side bias, to prevent it dominating too much.
if (nside)
side /= nside;
const float vpen = velWeight * (vdist(vcand, activeObst->dvel) * ivmax);
const float vcpen = curVelWeight * (vdist(vcand, activeObst->vel) * ivmax);
const float spen = sideWeight * side;
const float tpen = toiWeight * (1.0f/(0.1f+tmin/maxToi));
const float penalty = vpen + vcpen + spen + tpen;
if (penalty < minPenalty)
{
minPenalty = penalty;
vcpy(res, vcand);
}
}
}
