/* * 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); }