std::pair<Vec3, bool> RigidBodySimulation::intersect_ray(const Vec3& start, const Vec3& direction, float* distance, Vec3* normal) {
b3RayCastSingleShapeOutput result;
b3Vec3 s, d;
to_b3vec3(start, s);
to_b3vec3(start + direction, d);
bool hit = scene_->RayCastSingleShape(&result, s, d);
float closest = std::numeric_limits<float>::max();
Vec3 impact_point, closest_normal;
if(hit) {
to_vec3(result.point, impact_point);
closest = (impact_point - start).length();
to_vec3(result.normal, closest_normal);
if(distance) {
*distance = closest;
}
if(normal) {
*normal = closest_normal;
}
}
return std::make_pair(impact_point, hit);
}
void setup_vertex_normal_buffer_object(bool smoothed) {
std::vector<glm::vec3> vertices = trig.Vertices();
std::vector<glm::vec3> normals;
if (smoothed) {
// initialize map of normals to zero
// note that we store readily hashable vector<double> types instead of
// vec3s and convert between the two as required
// ...avoids some of the pain using <map> without much C++ knowledge
std::map< std::vector<double>, std::vector<double> > normal_map;
for (int i = 0; i < vertices.size(); i++) {
std::vector<double> zeros;
zeros.push_back(0.0);
zeros.push_back(0.0);
zeros.push_back(0.0);
normal_map[to_vector(vertices[i])] = zeros;
}
for (int i = 0; i < vertices.size(); i += 3) {
// get vertices of the current triangle
glm::vec3 v1 = vertices[i];
glm::vec3 v2 = vertices[i + 1];
glm::vec3 v3 = vertices[i + 2];
std::vector<double> v1_key = to_vector(v1);
std::vector<double> v2_key = to_vector(v2);
std::vector<double> v3_key = to_vector(v3);
// compute face normal
glm::vec3 face_normal = glm::cross(v3 - v2, v1 - v2);
// get the old vertex normal
glm::vec3 v1_old = to_vec3(normal_map[v1_key]);
glm::vec3 v2_old = to_vec3(normal_map[v2_key]);
glm::vec3 v3_old = to_vec3(normal_map[v3_key]);
// replace the old value with the new value
normal_map.erase(v1_key);
normal_map.erase(v2_key);
normal_map.erase(v3_key);
normal_map[v1_key] = to_vector(glm::normalize(v1_old + face_normal));
normal_map[v2_key] = to_vector(glm::normalize(v2_old + face_normal));
normal_map[v3_key] = to_vector(glm::normalize(v3_old + face_normal));
}
// convert the map of normals to a vector of normals
for (int i = 0; i < vertices.size(); i++) {
normals.push_back(to_vec3(normal_map[to_vector(vertices[i])]));
}
} else {
for (int i = 0; i < vertices.size(); i += 3) {
// get vertices of this triangle
glm::vec3 v1 = vertices[i];
glm::vec3 v2 = vertices[i + 1];
glm::vec3 v3 = vertices[i + 2];
// compute face normal
glm::vec3 face_normal = glm::cross(v3 - v2, v1 - v2);
normals.push_back(glm::normalize(face_normal));
normals.push_back(glm::normalize(face_normal));
normals.push_back(glm::normalize(face_normal));
}
}
glGenBuffers(1, &vertex_normal_buffer);
glBindBuffer(GL_ARRAY_BUFFER, vertex_normal_buffer);
glBufferData(GL_ARRAY_BUFFER, sizeof(glm::vec3) * normals.size(),
&normals[0], GL_STATIC_DRAW);
}
std::pair<Vec3, Quaternion> RigidBodySimulation::body_transform(const impl::Body *body) {
b3Body* b = bodies_.at(body);
auto position = b->GetWorldCenter();
auto rotation = b->GetOrientation();
Vec3 p;
to_vec3(position, p);
Quaternion r;
to_quat(rotation, r);
return std::make_pair(p, r);
}
void
MaterialParser::parse(std::istream& in)
{
bool default_program = true;
bool has_diffuse_texture = false;
bool has_specular_texture = false;
bool has_reflection_texture = false;
int current_texture_unit = 0;
std::string program_vertex = "src/glsl/default.vert";
std::string program_fragment = "src/glsl/default.frag";
std::vector<std::string> program_vertex_defines;
std::vector<std::string> program_fragment_defines;
m_material->enable(GL_CULL_FACE);
m_material->enable(GL_DEPTH_TEST);
m_material->set_uniform("material.ambient", glm::vec3(1.0f, 1.0f, 1.0f));
int line_number = 0;
std::string line;
while(std::getline(in, line))
{
try
{
line_number += 1;
std::vector<std::string> args = argument_parse(line);
if (!args.empty())
{
if (args[0] == "material.diffuse")
{
m_material->set_uniform("material.diffuse",
to_vec3(args.begin()+1, args.end(),
glm::vec3(1.0f, 1.0f, 1.0f)));
}
else if (args[0] == "material.diffuse_texture")
{
has_diffuse_texture = true;
if (args.size() == 2)
{
std::string diffuse_texture_name = to_string(args.begin()+1, args.end());
if (diffuse_texture_name == "buildin://video-texture")
{
m_material->set_video_texture(current_texture_unit);
}
else
{
m_material->set_texture(current_texture_unit, Texture::from_file(diffuse_texture_name));
}
}
else if (args.size() == 3)
{
m_material->set_texture(current_texture_unit,
Texture::from_file(args[1]),
Texture::from_file(args[2]));
}
else
{
throw std::runtime_error("broken");
}
m_material->set_uniform("material.diffuse_texture", current_texture_unit);
current_texture_unit += 1;
}
else if (args[0] == "material.specular")
{
m_material->set_uniform("material.specular",
to_vec3(args.begin()+1, args.end(),
glm::vec3(1.0f, 1.0f, 1.0f)));
}
else if (args[0] == "material.specular_texture")
{
has_specular_texture = true;
m_material->set_texture(current_texture_unit, Texture::from_file(to_string(args.begin()+1, args.end())));
m_material->set_uniform("material.specular_texture", current_texture_unit);
current_texture_unit += 1;
}
else if (args[0] == "material.shininess")
{
m_material->set_uniform("material.shininess",
to_float(args.begin()+1, args.end()));
}
else if (args[0] == "material.reflection_texture")
{
has_reflection_texture = true;
m_material->set_texture(current_texture_unit, Texture::cubemap_from_file(to_string(args.begin()+1, args.end())));
m_material->set_uniform("material.reflection_texture", current_texture_unit);
current_texture_unit += 1;
}
else if (args[0] == "material.ambient")
{
m_material->set_uniform("material.ambient",
to_vec3(args.begin()+1, args.end(),
glm::vec3(1.0f, 1.0f, 1.0f)));
}
else if (args[0] == "blend_mode")
{
//m_material->set_
}
else if (args[0] == "program.vertex")
{
program_vertex = args[1];
if (args.size() > 2)
{
program_vertex_defines.insert(program_vertex_defines.end(),
args.begin() + 2, args.end());
}
default_program = false;
}
else if (args[0] == "program.fragment")
{
program_fragment = args[1];
if (args.size() > 2)
{
program_fragment_defines.insert(program_fragment_defines.end(),
args.begin() + 2, args.end());
}
default_program = false;
}
else if (args[0] == "material.disable")
{
if (args[1] == "cull_face")
{
m_material->disable(GL_CULL_FACE);
}
else
{
throw std::runtime_error("unknown token: " + args[1]);
}
}
else if (boost::algorithm::starts_with(args[0], "uniform."))
{
std::string uniform_name = args[0].substr(8);
int count = args.end() - args.begin() - 1;
if (count == 3)
{
m_material->set_uniform(uniform_name,
to_vec3(args.begin()+1, args.end(),
glm::vec3(1.0f, 1.0f, 1.0f)));
}
else if (count == 1)
{
m_material->set_uniform(uniform_name, to_float(args.begin()+1, args.end()));
}
else
{
throw std::runtime_error("unknown argument count: " + args[0]);
}
}
else
{
throw std::runtime_error("unknown token: " + args[0]);
}
}
}
catch(const std::exception& err)
{
throw std::runtime_error(format("%s:%d: error: %s at line:\n%s", m_filename, line_number, err.what(), line));
}
}
if (default_program)
{
if (has_diffuse_texture)
{
program_fragment_defines.emplace_back("DIFFUSE_COLOR_FROM_TEXTURE");
}
else
{
program_fragment_defines.emplace_back("DIFFUSE_COLOR_FROM_MATERIAL");
}
if (has_specular_texture)
{
program_fragment_defines.emplace_back("SPECULAR_COLOR_FROM_TEXTURE");
}
else
{
program_fragment_defines.emplace_back("SPECULAR_COLOR_FROM_MATERIAL");
}
if (has_reflection_texture)
{
program_fragment_defines.emplace_back("REFLECTION_TEXTURE");
}
program_fragment_defines.emplace_back("SHADOW_VALUE_4");
}
ProgramPtr program = Program::create(Shader::from_file(GL_VERTEX_SHADER, program_vertex, program_vertex_defines),
Shader::from_file(GL_FRAGMENT_SHADER, program_fragment, program_fragment_defines));
m_material->set_program(program);
}
void drawLine(const btVector3 &from, const btVector3 &to,
const btVector3 &color) override
{
my_vertices.push_back(to_vec3(from));
my_vertices.push_back(to_vec3(to));
}
void Engine::update(float dt) {
// look //
double x, y;
glfwGetCursorPos(window, &x, &y);
static double last_x = x, last_y = y;
static float x_rot = glm::asin(camera.get_forward().y),
y_rot = glm::asin(camera.get_forward().x);
double dx = last_x - x, dy = last_y - y;
if (glfwGetKey(window, GLFW_KEY_K) == GLFW_PRESS) {
x_rot += dt * fake_mouse_change;
}
if (glfwGetKey(window, GLFW_KEY_J) == GLFW_PRESS) {
x_rot -= dt * fake_mouse_change;
}
if (glfwGetKey(window, GLFW_KEY_H) == GLFW_PRESS) {
y_rot -= dt * fake_mouse_change;
}
if (glfwGetKey(window, GLFW_KEY_L) == GLFW_PRESS) {
y_rot += dt * fake_mouse_change;
}
last_x = x;
last_y = y;
// positive y_rot means left.
y_rot -= mouse_sensitivity * dx;
x_rot += mouse_sensitivity * dy;
float max_angle = glm::radians(85.f);
float min_angle = -max_angle;
if (x_rot > max_angle) {
x_rot = max_angle;
} else if (x_rot < min_angle) {
x_rot = min_angle;
}
glm::vec3 forward = glm::vec3(
cos(x_rot) * sin(y_rot),
sin(x_rot),
-cos(x_rot) * cos(y_rot));
camera.lookat = camera.position + forward;
// movement //
glm::vec3 move_dir(0.f),
right = camera.get_right(),
up = glm::normalize(camera.up);
if (glfwGetKey(window, GLFW_KEY_W) == GLFW_PRESS) {
move_dir += forward;
}
if (glfwGetKey(window, GLFW_KEY_S) == GLFW_PRESS) {
move_dir -= forward;
}
if (glfwGetKey(window, GLFW_KEY_D) == GLFW_PRESS) {
move_dir += right;
}
if (glfwGetKey(window, GLFW_KEY_A) == GLFW_PRESS) {
move_dir -= right;
}
if (glfwGetKey(window, GLFW_KEY_E) == GLFW_PRESS) {
move_dir += up;
}
if (glfwGetKey(window, GLFW_KEY_Q) == GLFW_PRESS) {
move_dir -= up;
}
float move_speed = normal_move_speed;
if (glfwGetKey(window, GLFW_KEY_LEFT_SHIFT) == GLFW_PRESS) {
move_speed /= move_speed_change_factor;
}
if (glfwGetKey(window, GLFW_KEY_LEFT_CONTROL) == GLFW_PRESS) {
move_speed *= move_speed_change_factor;
}
float len = glm::length(move_dir);
if (len > 1.f) {
move_dir = move_dir / len;
}
if (glfwGetKey(window, GLFW_KEY_SPACE) == GLFW_PRESS) {
dt *= 10;
}
if (glfwGetKey(window, GLFW_KEY_V) == GLFW_PRESS) {
dt /= 10;
}
// Note: assumes dt will be >= 0. No timetravel...
glm::vec3 move = dt * move_speed * move_dir;
camera.position += move;
camera.lookat += move;
// physics //
bool paused = (glfwGetKey(window, GLFW_KEY_C) == GLFW_RELEASE);
if (!paused) {
physics->step(dt);
//auto printbtvec = [] (const char* fmt, btVector3 v) { // TODO
// printf(fmt, v.x(), v.y(), v.z());
//};
btScalar pi = SIMD_PI;
btVector3 local(0,0.2f,0);
btVector3 target = from_vec3(nodes["redcube"]->position);
KinematicChain* arm = roboarm3;
std::vector<btScalar> angles = arm->get_angles();
std::vector<btScalar> tar_angles =
arm->calc_angles_ccd(angles, target, local);
btVector3 dest = arm->get_position(tar_angles, local);
glm::vec3 offset = glm::vec3(0.05f, 0.f, 0.f);
nodes["cyancube"]->position = to_vec3(dest) - offset;
nodes["yellowcube"]->position = to_vec3(dest) + offset;
std::vector<btScalar> velocities(angles.size());
for (size_t i = 0; i < angles.size(); ++i) {
velocities[i] = (i+1)*7000.f*(tar_angles[i] - angles[i])*dt;
}
arm->apply_velocities(velocities);
//for (size_t i = 0; i < angles.size(); ++i) { // TODO
// printf("v[%lu] %f-%f ==> %f\n", i, angles[i], tar_angles[i],
// velocities[i]);
//}
}
if (glfwGetKey(window, GLFW_KEY_KP_ADD) == GLFW_PRESS) {
nodes["redcube"]->position +=
move_speed * dt * glm::vec3(0.f, 1.f, 0.f);
}
if (glfwGetKey(window, GLFW_KEY_KP_SUBTRACT) == GLFW_PRESS) {
nodes["redcube"]->position +=
move_speed * dt * glm::vec3(0.f, -1.f, 0.f);
}
if (glfwGetKey(window, GLFW_KEY_KP_2) == GLFW_PRESS) {
nodes["redcube"]->position +=
move_speed * dt * glm::vec3(0.f, 0.f, 1.f);
}
if (glfwGetKey(window, GLFW_KEY_KP_8) == GLFW_PRESS) {
nodes["redcube"]->position +=
move_speed * dt * glm::vec3(0.f, 0.f, -1.f);
}
if (glfwGetKey(window, GLFW_KEY_KP_4) == GLFW_PRESS) {
nodes["redcube"]->position +=
move_speed * dt * glm::vec3(-1.f, 0.f, 0.f);
}
if (glfwGetKey(window, GLFW_KEY_KP_6) == GLFW_PRESS) {
nodes["redcube"]->position +=
move_speed * dt * glm::vec3(1.f, 0.f, 0.f);
}
redcube_rb->setMotionState(redcube_rb->getMotionState());
nodes["cyancube"]->orientation *=
glm::angleAxis(dt*glm::radians(90.f),
glm::normalize(glm::vec3(1.f, 1.f, 1.f)));
// quit //
if (glfwGetKey(window, GLFW_KEY_ESCAPE) == GLFW_PRESS) {
glfwSetWindowShouldClose(window, GL_TRUE);
}
}
