/*
* Fragment shader for surface lighting. One directional light source, phong shading.
*/
static void fragment_shader_light(int y, int x, struct fragment_input *input, struct uniform_variables *vars){
if(input->frag_coord[VAR_Z] >= read_depth(y, x)){
return;
}
float *light_dir = &(vars->uniform_float[0]);
vec3 eye_normal, object_normal;
object_normal[VAR_X] = input->attributes[0] / input->frag_coord[VAR_W];
object_normal[VAR_Y] = input->attributes[1] / input->frag_coord[VAR_W];
object_normal[VAR_Z] = input->attributes[2] / input->frag_coord[VAR_W];
eye_normal[VAR_X] = input->attributes[3] / input->frag_coord[VAR_W];
eye_normal[VAR_Y] = input->attributes[4] / input->frag_coord[VAR_W];
eye_normal[VAR_Z] = input->attributes[5] / input->frag_coord[VAR_W];
normalize_vec3(eye_normal);
normalize_vec3(object_normal);
if(input->front_facing == ER_FALSE){
eye_normal[VAR_X] = -eye_normal[VAR_X];
eye_normal[VAR_Y] = -eye_normal[VAR_Y];
eye_normal[VAR_Z] = -eye_normal[VAR_Z];
object_normal[VAR_X] = -object_normal[VAR_X];
object_normal[VAR_Y] = -object_normal[VAR_Y];
object_normal[VAR_Z] = -object_normal[VAR_Z];
}
float diffuse_term = max(0.0f, dot_vec3(light_dir, eye_normal) );
float specular_term = 0.0f;
vec3 view_vector;
vec3 half_vector;
view_vector[VAR_X] = 0.0f;
view_vector[VAR_Y] = 0.0f;
view_vector[VAR_Z] = 1.0f;
if(diffuse_term > 0.0f){
half_vector[VAR_X] = light_dir[VAR_X] + view_vector[VAR_X];
half_vector[VAR_Y] = light_dir[VAR_Y] + view_vector[VAR_Y];
half_vector[VAR_Z] = light_dir[VAR_Z] + view_vector[VAR_Z];
normalize_vec3(half_vector);
specular_term = pow( max(0.0f, dot_vec3(half_vector, eye_normal)), 50.0f );
}
float red, green, blue, light_intensity;
light_intensity = 0.6f * diffuse_term + 0.4f * specular_term;
red = (object_normal[VAR_X] * 0.5f + 0.5f) * light_intensity;
red = clamp(red, 0.0f, 1.0f);
green = (object_normal[VAR_Y] * 0.5f + 0.5f) * light_intensity;
green = clamp(green, 0.0f, 1.0f);
blue = (object_normal[VAR_Z] * 0.5f + 0.5f) * light_intensity;
blue = clamp(blue, 0.0f, 1.0f);
write_color(y, x, red, green, blue, 1.0f);
write_depth(y, x, input->frag_coord[VAR_Z]);
}
void rotation_mat4(mat4 *m, scalar a, scalar u0, scalar u1, scalar u2)
{
vec3 u = {u0, u1, u2};
float sin = sinf((a * M_PI) / 180.0f);
float cos = cosf((a * M_PI) / 180.0f);
normalize_vec3(&u);
(*m)[0] = cos + u[0] * u[0] * (1.0f - cos);
(*m)[1] = u[1] * u[0] * (1.0f - cos) + u[2] * sin;
(*m)[2] = u[2] * u[0] * (1.0f - cos) - u[1] * sin;
(*m)[3] = 0.0;
(*m)[4] = u[0] * u[1] * (1.0f - cos) - u[2] * sin;
(*m)[5] = cos + u[1] * u[1] * (1.0f - cos);
(*m)[6] = u[2] * u[1] * (1.0f - cos) + u[0] * sin;
(*m)[7] = 0.0;
(*m)[8] = u[0] * u[2] * (1.0f - cos) + u[1] * sin;
(*m)[9] = u[1] * u[2] * (1.0f - cos) - u[0] * sin;
(*m)[10] = cos + u[2] * u[2] * (1.0f - cos);
(*m)[11] = 0.0;
(*m)[12] = 0.0;
(*m)[13] = 0.0;
(*m)[14] = 0.0;
(*m)[15] = 1.0;
}
/*
* Fragment shader for surface color based on surface normals.
*/
static void fragment_shader_color(int y, int x, struct fragment_input *input, struct uniform_variables *vars){
if(input->frag_coord[VAR_Z] >= read_depth(y, x)){
return;
}
vec3 object_normal;
object_normal[VAR_X] = input->attributes[0] / input->frag_coord[VAR_W];
object_normal[VAR_Y] = input->attributes[1] / input->frag_coord[VAR_W];
object_normal[VAR_Z] = input->attributes[2] / input->frag_coord[VAR_W];
normalize_vec3(object_normal);
if(input->front_facing == ER_FALSE){
object_normal[VAR_X] = -object_normal[VAR_X];
object_normal[VAR_Y] = -object_normal[VAR_Y];
object_normal[VAR_Z] = -object_normal[VAR_Z];
}
float red, green, blue;
red = (object_normal[VAR_X] * 0.5f + 0.5f);
red = clamp(red, 0.0f, 1.0f);
green = (object_normal[VAR_Y] * 0.5f + 0.5f);
green = clamp(green, 0.0f, 1.0f);
blue = (object_normal[VAR_X] * 0.5f + 0.5f);
blue = clamp(blue, 0.0f, 1.0f);
write_color(y, x, red, green, blue, 1.0f);
write_depth(y, x, input->frag_coord[VAR_Z]);
}
/*
* Evaluate surface equations and calculate surface normals.
*/
static void calculate_surface(){
int i = 0, j = 0;
float range_s = max_s - min_s;
float range_t = max_t - min_t;
struct var *var_s = &parser_vars[0], *var_t = &parser_vars[1];
var_s->value = 0.0f;
var_t->value = 0.0f;
int height = samples_s;
int width = samples_t;
float step_s, step_t;
step_s = range_s / (float)(samples_s-1);
step_t = range_t / (float)(samples_t-1);
float dFds[3], dFdt[3];
float next[3], prev[3], normal[3];
for(i = 0; i < height;i++){
for(j = 0; j < width;j++){
var_s->value = min_s + (float)i * step_s;
var_t->value = min_t + (float)j * step_t;
points[i*width*6 + j*6 ] = eval(exp_x);
points[i*width*6 + j*6 + 1] = eval(exp_y);
points[i*width*6 + j*6 + 2] = eval(exp_z);
// DFds
var_s->value = min_s + (float)(i+1) * step_s;
next[VAR_X] = eval(exp_x);
next[VAR_Y] = eval(exp_y);
next[VAR_Z] = eval(exp_z);
var_s->value = min_s + (float)(i-1) * step_s;
prev[VAR_X] = eval(exp_x);
prev[VAR_Y] = eval(exp_y);
prev[VAR_Z] = eval(exp_z);
dFds[VAR_X] = (next[VAR_X] - prev[VAR_X]) / (2.0f * step_s);
dFds[VAR_Y] = (next[VAR_Y] - prev[VAR_Y]) / (2.0f * step_s);
dFds[VAR_Z] = (next[VAR_Z] - prev[VAR_Z]) / (2.0f * step_s);
// DFdt
var_s->value = min_s + (float)i * step_s;
var_t->value = min_t + (float)(j+1) * step_t;
next[VAR_X] = eval(exp_x);
next[VAR_Y] = eval(exp_y);
next[VAR_Z] = eval(exp_z);
var_t->value = min_t + (float)(j-1) * step_t;
prev[VAR_X] = eval(exp_x);
prev[VAR_Y] = eval(exp_y);
prev[VAR_Z] = eval(exp_z);
dFdt[VAR_X] = (next[VAR_X] - prev[VAR_X]) / (2.0f * step_t);
dFdt[VAR_Y] = (next[VAR_Y] - prev[VAR_Y]) / (2.0f * step_t);
dFdt[VAR_Z] = (next[VAR_Z] - prev[VAR_Z]) / (2.0f * step_t);
// dFds x dFdt
cross_product(dFds, dFdt, normal);
normalize_vec3(normal);
points[i*width*6 + j*6 + 3] = normal[VAR_X];
points[i*width*6 + j*6 + 4] = normal[VAR_Y];
points[i*width*6 + j*6 + 5] = normal[VAR_Z];
}
}
}
static inline vec clamp(const vec& input, const double maxMag) {
vec v = input;
assert(v.n_elem % 3 == 0);
for (size_t i = 0; i < v.n_elem / 3; i++) {
vec3 cur = v.rows(3 * i, 3 * i + 2);
if (norm(cur, 2) > maxMag) {
normalize_vec3(cur);
cur = maxMag * cur;
v.rows(3 * i, 3 * i + 2) = cur;
}
}
return v;
}
mat& LinkedTreeRobot::computeConstraintJacobian(const vec& desired,
const vec& axes,
mat& m) const {
vector<TreeNode*> constraintNodes;
for (vector<TreeNode*>::const_iterator it = _allNodes.begin();
it != _allNodes.end();
++it)
if ((*it)->isFixed())
constraintNodes.push_back(*it);
m.zeros(3 * constraintNodes.size(), _numJoints);
for (size_t idx = 0; idx < constraintNodes.size(); idx++) {
TreeNode *node = constraintNodes[idx];
Context ctx(_rootPosition);
node->getContextForNode(ctx);
vec3 currentPos = ctx.getCurrentOrigin();
vec3 constraintError = node->getFixedPosition() - currentPos;
vec3 dCdP = -2.0 * constraintError;
TreeNode *curNode = node;
while (!curNode->isRootNode()) {
//cout << "currentNode: " << curNode->getIdentifier() << endl;
LinkState* linkState = curNode->getLinkState();
vector<size_t> jointIds = linkState->getJointIdentifiers();
assert(jointIds.size() == linkState->dof());
ctx.popContext();
vec3 direction = currentPos - ctx.getCurrentOrigin();
vec3 normDir = direction;
normalize_vec3(normDir);
for (size_t jointIdx = 0; jointIdx < linkState->dof(); jointIdx++) {
size_t jointId = jointIds[jointIdx];
vec3 rotAxis = axes.rows(3 * jointId, 3 * jointId + 2);
vec3 dPdq;
if (vec3_equals(rotAxis, normDir)) // HACK: this is how we do constraints for translation joints
dPdq = rotAxis;
else
dPdq = cross(rotAxis, direction);
vec3 jacEntry = dCdP % dPdq;
m.submat(3 * idx, jointId, 3 * idx + 2, jointId) = jacEntry;
}
curNode = curNode->getParent();
}
}
return m;
}
void rotation_mat3(mat3 *m, scalar a, scalar u0, scalar u1, scalar u2)
{
vec3 u = {u0, u1, u2};
scalar sin = sinf(a * M_PI / 180.0f);
scalar cos = cosf(a * M_PI / 180.0f);
normalize_vec3(&u);
(*m)[0] = cos + u[0] * u[0] * (1.0f - cos);
(*m)[1] = u[1] * u[0] * (1.0f - cos) + u[2] * sin;
(*m)[2] = u[2] * u[0] * (1.0f - cos) - u[1] * sin;
(*m)[3] = u[0] * u[1] * (1.0f - cos) - u[2] * sin;
(*m)[4] = cos + u[1] * u[1] * (1.0f - cos);
(*m)[5] = u[2] * u[1] * (1.0f - cos) + u[0] * sin;
(*m)[6] = u[0] * u[2] * (1.0f - cos) + u[1] * sin;
(*m)[7] = u[1] * u[2] * (1.0f - cos) - u[0] * sin;
(*m)[8] = cos + u[2] * u[2] * (1.0f - cos);
}
