int main () {
// start GL context with helper libraries
assert (glfwInit ());
/* We must specify 3.2 core if on Apple OS X -- other O/S can specify
anything here. I defined 'APPLE' in the makefile for OS X */
#ifdef APPLE
glfwWindowHint (GLFW_CONTEXT_VERSION_MAJOR, 3);
glfwWindowHint (GLFW_CONTEXT_VERSION_MINOR, 2);
glfwWindowHint (GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE);
glfwWindowHint (GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);
#endif
GLFWwindow* window = glfwCreateWindow (
g_viewport_width, g_viewport_height, "GUI Panels", NULL, NULL);
glfwSetWindowSizeCallback (window, glfw_window_size_callback);
glfwMakeContextCurrent (window);
glewExperimental = GL_TRUE;
glewInit ();
const GLubyte* renderer = glGetString (GL_RENDERER); // get renderer string
const GLubyte* version = glGetString (GL_VERSION); // version as a string
printf ("Renderer: %s\n", renderer);
printf ("OpenGL version supported %s\n", version);
// create a 2d panel. from 2 triangles = 6 xy coords.
float points[] = {
-1.0f, -1.0f,
1.0f, -1.0f,
-1.0f, 1.0f,
-1.0f, 1.0f,
1.0f, -1.0f,
1.0f, 1.0f
};
// for ground plane we can just re-use panel points but y is now z
GLuint vp_vbo, vao;
glGenBuffers (1, &vp_vbo);
glBindBuffer (GL_ARRAY_BUFFER, vp_vbo);
glBufferData (GL_ARRAY_BUFFER, sizeof (points), points, GL_STATIC_DRAW);
glGenVertexArrays (1, &vao);
glBindVertexArray (vao);
// note: vertex buffer is already bound
glVertexAttribPointer (0, 2, GL_FLOAT, GL_FALSE, 0, NULL);
glEnableVertexAttribArray (0);
// create a 3d camera to move in 3d so that we can tell that the panel is 2d
// keep track of some useful vectors that can be used for keyboard movement
vec4 fwd (0.0f, 0.0f, -1.0f, 0.0f);
vec4 rgt (1.0f, 0.0f, 0.0f, 0.0f);
vec4 up (0.0f, 1.0f, 0.0f, 0.0f);
vec3 cam_pos (0.0f, 1.0f, 5.0f);
mat4 T_inv = translate (identity_mat4 (), cam_pos);
// point slightly downwards to see the plane
versor quaternion = quat_from_axis_deg (0.0f, 1.0f, 0.0f, 0.0f);
mat4 R_inv = quat_to_mat4 (quaternion);
// combine the inverse rotation and transformation to make a view matrix
V = inverse (R_inv) * inverse (T_inv);
// projection matrix
P = perspective (
67.0f, (float)g_viewport_width / (float)g_viewport_height, 0.1f, 100.0f);
const float cam_speed = 3.0f; // 1 unit per second
const float cam_heading_speed = 50.0f; // 30 degrees per second
create_ground_plane_shaders ();
create_gui_shaders ();
// textures for ground plane and gui
GLuint gp_tex, gui_tex;
assert (load_texture ("tile2-diamonds256x256.png", &gp_tex));
assert (load_texture ("skulluvmap.png", &gui_tex));
// rendering defaults
glDepthFunc (GL_LESS); // set depth function but don't enable yet
glEnable (GL_CULL_FACE); // cull face
glCullFace (GL_BACK); // cull back face
glFrontFace (GL_CCW); // GL_CCW for counter clock-wise
// absolute panel dimensions in pixels
const float panel_width = 256.0f;
const float panel_height = 256.0f;
glViewport (0, 0, g_viewport_width, g_viewport_height);
// start main rendering loop
while (!glfwWindowShouldClose (window)) {
// update timers
static double previous_seconds = glfwGetTime ();
double current_seconds = glfwGetTime ();
double elapsed_seconds = current_seconds - previous_seconds;
previous_seconds = current_seconds;
bool cam_moved = false;
vec3 move (0.0, 0.0, 0.0);
// wipe the drawing surface clear
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// draw ground plane. note: depth test is enabled here
glEnable (GL_DEPTH_TEST);
glActiveTexture (GL_TEXTURE0);
glBindTexture (GL_TEXTURE_2D, gp_tex);
glUseProgram (gp_sp);
glBindVertexArray (vao);
glDrawArrays (GL_TRIANGLES, 0, 6);
// draw GUI panel. note: depth test is disabled here and drawn AFTER scene
glDisable (GL_DEPTH_TEST);
glActiveTexture (GL_TEXTURE0);
glBindTexture (GL_TEXTURE_2D, gui_tex);
glUseProgram (gui_sp);
// resize panel to size in pixels
float x_scale = panel_width / g_viewport_width;
float y_scale = panel_height / g_viewport_height;
glUniform2f (gui_scale_loc, x_scale, y_scale);
glBindVertexArray (vao);
glDrawArrays (GL_TRIANGLES, 0, 6);
// update other events like input handling
glfwPollEvents ();
if (GLFW_PRESS == glfwGetKey (window, GLFW_KEY_ESCAPE)) {
glfwSetWindowShouldClose (window, 1);
}
float cam_yaw = 0.0f; // y-rotation in degrees
float cam_pitch = 0.0f;
float cam_roll = 0.0;
if (glfwGetKey (window, GLFW_KEY_A)) {
move.v[0] -= cam_speed * elapsed_seconds;
cam_moved = true;
}
if (glfwGetKey (window, GLFW_KEY_D)) {
move.v[0] += cam_speed * elapsed_seconds;
cam_moved = true;
}
if (glfwGetKey (window, GLFW_KEY_Q)) {
move.v[1] += cam_speed * elapsed_seconds;
cam_moved = true;
}
if (glfwGetKey (window, GLFW_KEY_E)) {
move.v[1] -= cam_speed * elapsed_seconds;
cam_moved = true;
}
if (glfwGetKey (window, GLFW_KEY_W)) {
move.v[2] -= cam_speed * elapsed_seconds;
cam_moved = true;
}
if (glfwGetKey (window, GLFW_KEY_S)) {
move.v[2] += cam_speed * elapsed_seconds;
cam_moved = true;
}
if (glfwGetKey (window, GLFW_KEY_LEFT)) {
cam_yaw += cam_heading_speed * elapsed_seconds;
cam_moved = true;
versor q_yaw = quat_from_axis_deg (cam_yaw, up.v[0], up.v[1], up.v[2]);
quaternion = q_yaw * quaternion;
}
if (glfwGetKey (window, GLFW_KEY_RIGHT)) {
cam_yaw -= cam_heading_speed * elapsed_seconds;
cam_moved = true;
versor q_yaw = quat_from_axis_deg (cam_yaw, up.v[0], up.v[1], up.v[2]);
quaternion = q_yaw * quaternion;
}
if (glfwGetKey (window, GLFW_KEY_UP)) {
cam_pitch += cam_heading_speed * elapsed_seconds;
cam_moved = true;
versor q_pitch = quat_from_axis_deg (
cam_pitch, rgt.v[0], rgt.v[1], rgt.v[2]);
quaternion = q_pitch * quaternion;
}
if (glfwGetKey (window, GLFW_KEY_DOWN)) {
cam_pitch -= cam_heading_speed * elapsed_seconds;
cam_moved = true;
versor q_pitch = quat_from_axis_deg (
cam_pitch, rgt.v[0], rgt.v[1], rgt.v[2]);
quaternion = q_pitch * quaternion;
}
if (glfwGetKey (window, GLFW_KEY_Z)) {
cam_roll -= cam_heading_speed * elapsed_seconds;
cam_moved = true;
versor q_roll = quat_from_axis_deg (
cam_roll, fwd.v[0], fwd.v[1], fwd.v[2]);
quaternion = q_roll * quaternion;
}
if (glfwGetKey (window, GLFW_KEY_C)) {
cam_roll += cam_heading_speed * elapsed_seconds;
cam_moved = true;
versor q_roll = quat_from_axis_deg (
cam_roll, fwd.v[0], fwd.v[1], fwd.v[2]);
quaternion = q_roll * quaternion;
}
// update view matrix
if (cam_moved) {
// re-calculate local axes so can move fwd in dir cam is pointing
R_inv = quat_to_mat4 (quaternion);
fwd = R_inv * vec4 (0.0, 0.0, -1.0, 0.0);
rgt = R_inv * vec4 (1.0, 0.0, 0.0, 0.0);
up = R_inv * vec4 (0.0, 1.0, 0.0, 0.0);
cam_pos = cam_pos + vec3 (fwd) * -move.v[2];
cam_pos = cam_pos + vec3 (up) * move.v[1];
cam_pos = cam_pos + vec3 (rgt) * move.v[0];
T_inv = translate (identity_mat4 (), cam_pos);
V = inverse (R_inv) * inverse (T_inv);
glUseProgram (gp_sp);
glUniformMatrix4fv (gp_V_loc, 1, GL_FALSE, V.m);
}
// put the stuff we've been drawing onto the display
glfwSwapBuffers (window);
}
// done
glfwTerminate();
return 0;
}
const Point3 WGS84Coordinate::transform(const WGS84Coordinate &coordinate) const {
pair<double, double> result = fwd(coordinate.getLatitude() * Constants::DEG2RAD, coordinate.getLongitude() * Constants::DEG2RAD);
return Point3(result.first, result.second, 0);
}
int main () {
// start GL context and O/S window using the GLFW helper library
if (!glfwInit ()) {
fprintf (stderr, "ERROR: could not start GLFW3\n");
return 1;
}
// uncomment these lines if on Apple OS X
glfwWindowHint (GLFW_CONTEXT_VERSION_MAJOR, 3);
glfwWindowHint (GLFW_CONTEXT_VERSION_MINOR, 2);
glfwWindowHint (GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE);
glfwWindowHint (GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE);
GLFWwindow* window = glfwCreateWindow (640, 480, "Cube to Sphere", NULL, NULL);
if (!window) {
fprintf (stderr, "ERROR: could not open window with GLFW3\n");
glfwTerminate();
return 1;
}
GLFWwindow* window2 = glfwCreateWindow (640, 480, "Just checking", NULL, NULL);
if (!window2) {
fprintf (stderr, "ERROR: could not open window with GLFW3\n");
glfwTerminate();
return 1;
}
glfwMakeContextCurrent (window);
//start GLEW extension handler
glewExperimental = GL_TRUE;
glewInit ();
// get version info
const GLubyte* renderer = glGetString (GL_RENDERER); // get renderer string
const GLubyte* version = glGetString (GL_VERSION); // version as a string
printf ("Renderer: %s\n", renderer);
printf ("OpenGL version supported %s\n", version);
//CLGLUtils::init();
boost::compute::context context;
try {
context = boost::compute::opengl_create_shared_context();
} catch (std::exception e) {
std::cerr<<"Failed to initialize a CLGL context"<<e.what()<<std::endl;
exit(0);
}
// tell GL to only draw onto a pixel if the shape is closer to the viewer
glEnable (GL_DEPTH_TEST); // enable depth-testing
glDepthFunc (GL_LESS); // depth-testing interprets a smaller value as "closer"
// OTHER STUFF GOES HERE NEXT
float points[] = {
-1.0f, 1.0f, -1.0f,
-1.0f, -1.0f, -1.0f,
1.0f, -1.0f, -1.0f,
1.0f, -1.0f, -1.0f,
1.0f, 1.0f, -1.0f,
-1.0f, 1.0f, -1.0f,
-1.0f, -1.0f, 1.0f,
-1.0f, -1.0f, -1.0f,
-1.0f, 1.0f, -1.0f,
-1.0f, 1.0f, -1.0f,
-1.0f, 1.0f, 1.0f,
-1.0f, -1.0f, 1.0f,
1.0f, -1.0f, -1.0f,
1.0f, -1.0f, 1.0f,
1.0f, 1.0f, 1.0f,
1.0f, 1.0f, 1.0f,
1.0f, 1.0f, -1.0f,
1.0f, -1.0f, -1.0f,
-1.0f, -1.0f, 1.0f,
-1.0f, 1.0f, 1.0f,
1.0f, 1.0f, 1.0f,
1.0f, 1.0f, 1.0f,
1.0f, -1.0f, 1.0f,
-1.0f, -1.0f, 1.0f,
-1.0f, 1.0f, -1.0f,
1.0f, 1.0f, -1.0f,
1.0f, 1.0f, 1.0f,
1.0f, 1.0f, 1.0f,
-1.0f, 1.0f, 1.0f,
-1.0f, 1.0f, -1.0f,
-1.0f, -1.0f, -1.0f,
-1.0f, -1.0f, 1.0f,
1.0f, -1.0f, -1.0f,
1.0f, -1.0f, -1.0f,
-1.0f, -1.0f, 1.0f,
1.0f, -1.0f, 1.0f
};
GLuint vbo;
glGenBuffers (1, &vbo);
glBindBuffer (GL_ARRAY_BUFFER, vbo);
glBufferData (GL_ARRAY_BUFFER, 3 * 36 * sizeof (float), &points, GL_STATIC_DRAW);
GLuint vao;
glGenVertexArrays (1, &vao);
glBindVertexArray (vao);
glEnableVertexAttribArray (0);
glBindBuffer (GL_ARRAY_BUFFER, vbo);
glVertexAttribPointer (0, 3, GL_FLOAT, GL_FALSE, 0, NULL);
//Create Cube map
GLuint cube_map_texture;
create_cube_map ((resource_dir+"negz.jpg").c_str(), (resource_dir+"posz.jpg").c_str(), (resource_dir+"posy.jpg").c_str(), (resource_dir+"negy.jpg").c_str(), (resource_dir+"negx.jpg").c_str(), (resource_dir+"posx.jpg").c_str(), &cube_map_texture);
boost::shared_ptr<boost::compute::opengl_texture> cl_cube_map_texture;
try {
cl_cube_map_texture = boost::shared_ptr<boost::compute::opengl_texture>(new boost::compute::opengl_texture(context,GL_TEXTURE_2D_ARRAY,0,cube_map_texture,boost::compute::memory_object::mem_flags::read_only));
} catch ( const std::exception& e ) {
std::cerr << e.what() << std::endl;
}
const char* vertex_shader =
"#version 400\n"
"in vec3 vp;"
"uniform mat4 P, V;"
// "uniform vec3 vertOut;"
"out vec3 texcoords;"
"vec3 newP;"
"void main () {"
" texcoords = vp;"
// " vertOut = vp;"
" gl_Position = P * V * vec4 (vp, 1.0);"
"}";
const char* fragment_shader = loadShader(resource_dir+"fragmentShader.frag").c_str();
/*"#version 400\n"
"in vec3 texcoords;"
"uniform samplerCube cube_texture;"
"out vec4 frag_colour;"
"vec4 cubeToLatLon(samplerCube cubemap, vec3 inUV) {"
"vec3 cubmapTexCoords;"
//"cubmapTexCoords.x = inUV.x*sqrt(1 - ( (inUV.y * inUV.y)/2 ) - ( (inUV.z * inUV.z)/2 ) + ( ( (inUV.y * inUV.y) * (inUV.z * inUV.z))/3));"
"cubmapTexCoords.x = inUV.x;"
//"cubmapTexCoords.y= inUV.y*sqrt(1 - ( (inUV.z * inUV.z)/2 ) - ( (inUV.x * inUV.x)/2 ) + ( ( (inUV.z * inUV.z) * (inUV.x * inUV.x))/3));"
"cubmapTexCoords.y = inUV.y;"
"cubmapTexCoords.z = inUV.z*sqrt(1 - ( (inUV.x * inUV.x)/2 ) - ( (inUV.y * inUV.y)/2 ) + ( ( (inUV.x * inUV.x) * (inUV.y * inUV.y))/3));"
//"cubmapTexCoords.z = inUV.z;"
"return texture(cubemap, cubmapTexCoords);"
"}"
"void main () {"
//" frag_colour = texture (cube_texture, texcoords);"
" frag_colour = cubeToLatLon (cube_texture, texcoords);"
"}";*/
GLuint vs = glCreateShader (GL_VERTEX_SHADER);
glShaderSource (vs, 1, &vertex_shader, NULL);
glCompileShader (vs);
GLuint fs = glCreateShader (GL_FRAGMENT_SHADER);
glShaderSource (fs, 1, &fragment_shader, NULL);
glCompileShader (fs);
GLuint cube_sp = glCreateProgram ();
glAttachShader (cube_sp, fs);
glAttachShader (cube_sp, vs);
glLinkProgram (cube_sp);
//*-----------------------------Compile Shaders for second window - square --------*/
glfwMakeContextCurrent(window2);
//start GLEW extension handler
glewExperimental = GL_TRUE;
glewInit ();
glEnable (GL_DEPTH_TEST); // enable depth-testing
glDepthFunc (GL_LESS); // depth-testing interprets a smaller value as "closer"
float squarePoints[] =
{
-1.0f, 1.0f, -1.0f,
-1.0f, -1.0f, -1.0f,
1.0f, -1.0f, -1.0f,
1.0f, -1.0f, -1.0f,
1.0f, 1.0f, -1.0f,
-1.0f, 1.0f, -1.0f
};
GLfloat texcoords[] = {
0.0f, 0.0f,
1.0f, 0.0f,
1.0f, 1.0f,
1.0f, 1.0f,
0.0f, 1.0f,
0.0f, 0.0f
};
GLuint vbo_square;
glGenBuffers (1, &vbo_square);
glBindBuffer (GL_ARRAY_BUFFER, vbo_square);
glBufferData (GL_ARRAY_BUFFER, 3 * 6 * sizeof (float), &squarePoints, GL_STATIC_DRAW);
GLuint texcoords_vbo;
glGenBuffers (1, &texcoords_vbo);
glBindBuffer (GL_ARRAY_BUFFER, texcoords_vbo);
glBufferData (GL_ARRAY_BUFFER, 12 * sizeof (GLfloat), texcoords, GL_STATIC_DRAW);
GLuint vao_square;
glGenVertexArrays (1, &vao_square);
glBindVertexArray (vao_square);
glBindBuffer (GL_ARRAY_BUFFER, vbo_square);
glVertexAttribPointer (0, 3, GL_FLOAT, GL_FALSE, 0, NULL);
glBindBuffer (GL_ARRAY_BUFFER, texcoords_vbo);
glVertexAttribPointer (1, 2, GL_FLOAT, GL_FALSE, 0, NULL); // normalise!
glEnableVertexAttribArray (0);
glEnableVertexAttribArray (1);
GLuint square_sp = create_programme_from_files((resource_dir+"square.vert").c_str(), (resource_dir+"square.frag").c_str());
GLuint tex;
assert (load_texture ((resource_dir+"negz.jpg").c_str(), &tex));
//*----------------------------------------------------------------------------------*/
glfwMakeContextCurrent (window);
int cube_V_location = glGetUniformLocation (cube_sp, "V");
int cube_P_location = glGetUniformLocation (cube_sp, "P");
//int cube_vertOut = glGetUniformLocation (cube_sp, "vertOut");
/*-------------------------------CREATE GLOBAL CAMERA--------------------------------*/
#define ONE_DEG_IN_RAD (2.0 * M_PI) / 360.0 // 0.017444444
// input variables
float near = 0.1f; // clipping plane
float far = 100.0f; // clipping plane
float fovy = 80.0f; // 67 degrees
float aspect = (float)g_gl_width / (float)g_gl_height; // aspect ratio
proj_mat = perspective (fovy, aspect, near, far);
float cam_speed = 3.0f; // 1 unit per second
float cam_heading_speed = 50.0f; // 30 degrees per second
float cam_heading = 0.0f; // y-rotation in degrees
mat4 T = translate (identity_mat4 (), vec3 (-cam_pos.v[0], -cam_pos.v[1], -cam_pos.v[2]));
mat4 R = rotate_y_deg (identity_mat4 (), -cam_heading);
versor q = quat_from_axis_deg (-cam_heading, 0.0f, 1.0f, 0.0f);
view_mat = R * T;
// keep track of some useful vectors that can be used for keyboard movement
vec4 fwd (0.0f, 0.0f, -1.0f, 0.0f);
vec4 rgt (1.0f, 0.0f, 0.0f, 0.0f);
vec4 up (0.0f, 1.0f, 0.0f, 0.0f);
/*---------------------------SET RENDERING DEFAULTS---------------------------*/
glUseProgram (cube_sp);
glUniformMatrix4fv (cube_V_location, 1, GL_FALSE, R.m);
glUniformMatrix4fv (cube_P_location, 1, GL_FALSE, proj_mat.m);
// unique model matrix for each sphere
mat4 model_mat = identity_mat4 ();
glEnable (GL_DEPTH_TEST); // enable depth-testing
glDepthFunc (GL_LESS); // depth-testing interprets a smaller value as "closer"
glEnable (GL_CULL_FACE); // cull face
glCullFace (GL_BACK); // cull back face
glFrontFace (GL_CCW); // set counter-clock-wise vertex order to mean the front
glClearColor (0.2, 0.2, 0.2, 1.0); // grey background to help spot mistakes
glViewport (0, 0, g_gl_width, g_gl_height);
while (!glfwWindowShouldClose (window) && !glfwWindowShouldClose (window2)) {
// update timers
static double previous_seconds = glfwGetTime ();
double current_seconds = glfwGetTime ();
double elapsed_seconds = current_seconds - previous_seconds;
previous_seconds = current_seconds;
//_update_fps_counter (window);
// wipe the drawing surface clear
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// render a sky-box using the cube-map texture
glDepthMask (GL_FALSE);
glUseProgram (cube_sp);
glActiveTexture (GL_TEXTURE0);
glBindTexture (GL_TEXTURE_CUBE_MAP, cube_map_texture);
glBindVertexArray (vao);
glDrawArrays (GL_TRIANGLES, 0, 36);
glDepthMask (GL_TRUE);
//*---------------------------------Display for second window-------------------*/
glfwMakeContextCurrent (window2);
glUseProgram (square_sp);
int cubemap_vert = glGetUniformLocation (square_sp, "cubeMap_texcoords");
glEnable (GL_DEPTH_TEST); // enable depth-testing
glDepthFunc (GL_LESS); // depth-testing interprets a smaller value as "closer"
glEnable (GL_CULL_FACE); // cull face
glCullFace (GL_BACK); // cull back face
glFrontFace (GL_CCW); // set counter-clock-wise vertex order to mean the front
glClearColor (0.3, 0.2, 0.3, 1.0); // grey background to help spot mistakes
glViewport (0, 0, g_gl_width, g_gl_height);
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glDepthMask (GL_FALSE);
glUseProgram (square_sp);
glBindVertexArray (vao_square);
glDrawArrays (GL_TRIANGLES, 0, 6);
glDepthMask (GL_TRUE);
//*------------------------------GO back to cubemap window--------------------*/
glfwMakeContextCurrent (window);
// update other events like input handling
glfwPollEvents ();
// control keys
bool cam_moved = false;
vec3 move (0.0, 0.0, 0.0);
float cam_yaw = 0.0f; // y-rotation in degrees
float cam_pitch = 0.0f;
float cam_roll = 0.0;
if (glfwGetKey (window, GLFW_KEY_A)) {
move.v[0] -= cam_speed * elapsed_seconds;
cam_moved = true;
print(move);
}
if (glfwGetKey (window, GLFW_KEY_D)) {
move.v[0] += cam_speed * elapsed_seconds;
cam_moved = true;
}
if (glfwGetKey (window, GLFW_KEY_Q)) {
move.v[1] += cam_speed * elapsed_seconds;
cam_moved = true;
}
if (glfwGetKey (window, GLFW_KEY_E)) {
move.v[1] -= cam_speed * elapsed_seconds;
cam_moved = true;
}
if (glfwGetKey (window, GLFW_KEY_W)) {
move.v[2] -= cam_speed * elapsed_seconds;
cam_moved = true;
}
if (glfwGetKey (window, GLFW_KEY_S)) {
move.v[2] += cam_speed * elapsed_seconds;
cam_moved = true;
}
if (glfwGetKey (window, GLFW_KEY_LEFT)) {
cam_yaw += cam_heading_speed * elapsed_seconds;
cam_moved = true;
versor q_yaw = quat_from_axis_deg (
cam_yaw, up.v[0], up.v[1], up.v[2]
);
q = q_yaw * q;
}
if (glfwGetKey (window, GLFW_KEY_RIGHT)) {
cam_yaw -= cam_heading_speed * elapsed_seconds;
cam_moved = true;
versor q_yaw = quat_from_axis_deg (
cam_yaw, up.v[0], up.v[1], up.v[2]
);
q = q_yaw * q;
}
if (glfwGetKey (window, GLFW_KEY_UP)) {
cam_pitch += cam_heading_speed * elapsed_seconds;
cam_moved = true;
versor q_pitch = quat_from_axis_deg (
cam_pitch, rgt.v[0], rgt.v[1], rgt.v[2]
);
q = q_pitch * q;
}
if (glfwGetKey (window, GLFW_KEY_DOWN)) {
cam_pitch -= cam_heading_speed * elapsed_seconds;
cam_moved = true;
versor q_pitch = quat_from_axis_deg (
cam_pitch, rgt.v[0], rgt.v[1], rgt.v[2]
);
q = q_pitch * q;
}
if (glfwGetKey (window, GLFW_KEY_Z)) {
cam_roll -= cam_heading_speed * elapsed_seconds;
cam_moved = true;
versor q_roll = quat_from_axis_deg (
cam_roll, fwd.v[0], fwd.v[1], fwd.v[2]
);
q = q_roll * q;
}
if (glfwGetKey (window, GLFW_KEY_C)) {
cam_roll += cam_heading_speed * elapsed_seconds;
cam_moved = true;
versor q_roll = quat_from_axis_deg (
cam_roll, fwd.v[0], fwd.v[1], fwd.v[2]
);
q = q_roll * q;
}
// update view matrix
if (cam_moved) {
cam_heading += cam_yaw;
// re-calculate local axes so can move fwd in dir cam is pointing
R = quat_to_mat4 (q);
fwd = R * vec4 (0.0, 0.0, -1.0, 0.0);
rgt = R * vec4 (1.0, 0.0, 0.0, 0.0);
up = R * vec4 (0.0, 1.0, 0.0, 0.0);
cam_pos = cam_pos + vec3 (fwd) * -move.v[2];
cam_pos = cam_pos + vec3 (up) * move.v[1];
cam_pos = cam_pos + vec3 (rgt) * move.v[0];
mat4 T = translate (identity_mat4 (), vec3 (cam_pos));
view_mat = inverse (R) * inverse (T);
//std::cout<<inverse(R).m<<std::endl;
// cube-map view matrix has rotation, but not translation
glUseProgram (cube_sp);
glUniformMatrix4fv (cube_V_location, 1, GL_FALSE, inverse (R).m);
}
if (GLFW_PRESS == glfwGetKey (window, GLFW_KEY_ESCAPE)) {
glfwSetWindowShouldClose (window, 1);
}
if (GLFW_PRESS == glfwGetKey (window2, GLFW_KEY_ESCAPE)) {
glfwSetWindowShouldClose (window2, 1);
}
// put the stuff we've been drawing onto the display
glfwSwapBuffers (window);
glfwMakeContextCurrent (window2);
glfwSwapBuffers (window2);
glfwMakeContextCurrent (window);
}
// close GL context and any other GLFW resources
glfwTerminate();
return 0;
}
int main () {
/*--------------------------------START OPENGL--------------------------------*/
assert (restart_gl_log ());
// start GL context and O/S window using the GLFW helper library
assert (start_gl ());
// set a function to be called when the mouse is clicked
glfwSetMouseButtonCallback (g_window, glfw_mouse_click_callback);
/*------------------------------CREATE GEOMETRY-------------------------------*/
GLfloat* vp = NULL; // array of vertex points
GLfloat* vn = NULL; // array of vertex normals
GLfloat* vt = NULL; // array of texture coordinates
int g_point_count = 0;
assert (load_obj_file (MESH_FILE, vp, vt, vn, g_point_count));
GLuint vao;
glGenVertexArrays (1, &vao);
glBindVertexArray (vao);
GLuint points_vbo;
if (NULL != vp) {
glGenBuffers (1, &points_vbo);
glBindBuffer (GL_ARRAY_BUFFER, points_vbo);
glBufferData (
GL_ARRAY_BUFFER, 3 * g_point_count * sizeof (GLfloat), vp, GL_STATIC_DRAW
);
glVertexAttribPointer (0, 3, GL_FLOAT, GL_FALSE, 0, NULL);
glEnableVertexAttribArray (0);
}
/*-------------------------------CREATE SHADERS-------------------------------*/
GLuint shader_programme = create_programme_from_files (
VERTEX_SHADER_FILE, FRAGMENT_SHADER_FILE
);
int model_mat_location = glGetUniformLocation (shader_programme, "model");
int view_mat_location = glGetUniformLocation (shader_programme, "view");
int proj_mat_location = glGetUniformLocation (shader_programme, "proj");
int blue_location = glGetUniformLocation (shader_programme, "blue");
/*-------------------------------CREATE CAMERA--------------------------------*/
#define ONE_DEG_IN_RAD (2.0 * M_PI) / 360.0 // 0.017444444
// input variables
float near = 0.1f; // clipping plane
float far = 100.0f; // clipping plane
float fovy = 67.0f; // 67 degrees
float aspect = (float)g_gl_width / (float)g_gl_height; // aspect ratio
proj_mat = perspective (fovy, aspect, near, far);
float cam_speed = 3.0f; // 1 unit per second
float cam_heading_speed = 50.0f; // 30 degrees per second
float cam_heading = 0.0f; // y-rotation in degrees
mat4 T = translate (
identity_mat4 (), vec3 (-cam_pos.v[0], -cam_pos.v[1], -cam_pos.v[2])
);
mat4 R = rotate_y_deg (identity_mat4 (), -cam_heading);
versor q = quat_from_axis_deg (-cam_heading, 0.0f, 1.0f, 0.0f);
view_mat = R * T;
// keep track of some useful vectors that can be used for keyboard movement
vec4 fwd (0.0f, 0.0f, -1.0f, 0.0f);
vec4 rgt (1.0f, 0.0f, 0.0f, 0.0f);
vec4 up (0.0f, 1.0f, 0.0f, 0.0f);
/*---------------------------SET RENDERING DEFAULTS---------------------------*/
glUseProgram (shader_programme);
glUniformMatrix4fv (view_mat_location, 1, GL_FALSE, view_mat.m);
glUniformMatrix4fv (proj_mat_location, 1, GL_FALSE, proj_mat.m);
// unique model matrix for each sphere
mat4 model_mats[NUM_SPHERES];
for (int i = 0; i < NUM_SPHERES; i++) {
model_mats[i] = translate (identity_mat4 (), sphere_pos_wor[i]);
}
glEnable (GL_DEPTH_TEST); // enable depth-testing
glDepthFunc (GL_LESS); // depth-testing interprets a smaller value as "closer"
glEnable (GL_CULL_FACE); // cull face
glCullFace (GL_BACK); // cull back face
glFrontFace (GL_CCW); // set counter-clock-wise vertex order to mean the front
glClearColor (0.2, 0.2, 0.2, 1.0); // grey background to help spot mistakes
glViewport (0, 0, g_gl_width, g_gl_height);
/*-------------------------------RENDERING LOOP-------------------------------*/
while (!glfwWindowShouldClose (g_window)) {
// update timers
static double previous_seconds = glfwGetTime ();
double current_seconds = glfwGetTime ();
double elapsed_seconds = current_seconds - previous_seconds;
previous_seconds = current_seconds;
_update_fps_counter (g_window);
// wipe the drawing surface clear
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glUseProgram (shader_programme);
glBindVertexArray (vao);
for (int i = 0; i < NUM_SPHERES; i++) {
if (g_selected_sphere == i) {
glUniform1f (blue_location, 1.0f);
} else {
glUniform1f (blue_location, 0.0f);
}
glUniformMatrix4fv (model_mat_location, 1, GL_FALSE, model_mats[i].m);
glDrawArrays (GL_TRIANGLES, 0, g_point_count);
}
// update other events like input handling
glfwPollEvents ();
// control keys
bool cam_moved = false;
vec3 move (0.0, 0.0, 0.0);
float cam_yaw = 0.0f; // y-rotation in degrees
float cam_pitch = 0.0f;
float cam_roll = 0.0;
if (glfwGetKey (g_window, GLFW_KEY_A)) {
move.v[0] -= cam_speed * elapsed_seconds;
cam_moved = true;
}
if (glfwGetKey (g_window, GLFW_KEY_D)) {
move.v[0] += cam_speed * elapsed_seconds;
cam_moved = true;
}
if (glfwGetKey (g_window, GLFW_KEY_Q)) {
move.v[1] += cam_speed * elapsed_seconds;
cam_moved = true;
}
if (glfwGetKey (g_window, GLFW_KEY_E)) {
move.v[1] -= cam_speed * elapsed_seconds;
cam_moved = true;
}
if (glfwGetKey (g_window, GLFW_KEY_W)) {
move.v[2] -= cam_speed * elapsed_seconds;
cam_moved = true;
}
if (glfwGetKey (g_window, GLFW_KEY_S)) {
move.v[2] += cam_speed * elapsed_seconds;
cam_moved = true;
}
if (glfwGetKey (g_window, GLFW_KEY_LEFT)) {
cam_yaw += cam_heading_speed * elapsed_seconds;
cam_moved = true;
versor q_yaw = quat_from_axis_deg (
cam_yaw, up.v[0], up.v[1], up.v[2]
);
q = q_yaw * q;
}
if (glfwGetKey (g_window, GLFW_KEY_RIGHT)) {
cam_yaw -= cam_heading_speed * elapsed_seconds;
cam_moved = true;
versor q_yaw = quat_from_axis_deg (
cam_yaw, up.v[0], up.v[1], up.v[2]
);
q = q_yaw * q;
}
if (glfwGetKey (g_window, GLFW_KEY_UP)) {
cam_pitch += cam_heading_speed * elapsed_seconds;
cam_moved = true;
versor q_pitch = quat_from_axis_deg (
cam_pitch, rgt.v[0], rgt.v[1], rgt.v[2]
);
q = q_pitch * q;
}
if (glfwGetKey (g_window, GLFW_KEY_DOWN)) {
cam_pitch -= cam_heading_speed * elapsed_seconds;
cam_moved = true;
versor q_pitch = quat_from_axis_deg (
cam_pitch, rgt.v[0], rgt.v[1], rgt.v[2]
);
q = q_pitch * q;
}
if (glfwGetKey (g_window, GLFW_KEY_Z)) {
cam_roll -= cam_heading_speed * elapsed_seconds;
cam_moved = true;
versor q_roll = quat_from_axis_deg (
cam_roll, fwd.v[0], fwd.v[1], fwd.v[2]
);
q = q_roll * q;
}
if (glfwGetKey (g_window, GLFW_KEY_C)) {
cam_roll += cam_heading_speed * elapsed_seconds;
cam_moved = true;
versor q_roll = quat_from_axis_deg (
cam_roll, fwd.v[0], fwd.v[1], fwd.v[2]
);
q = q_roll * q;
}
// update view matrix
if (cam_moved) {
// re-calculate local axes so can move fwd in dir cam is pointing
R = quat_to_mat4 (q);
fwd = R * vec4 (0.0, 0.0, -1.0, 0.0);
rgt = R * vec4 (1.0, 0.0, 0.0, 0.0);
up = R * vec4 (0.0, 1.0, 0.0, 0.0);
cam_pos = cam_pos + vec3 (fwd) * -move.v[2];
cam_pos = cam_pos + vec3 (up) * move.v[1];
cam_pos = cam_pos + vec3 (rgt) * move.v[0];
mat4 T = translate (identity_mat4 (), vec3 (cam_pos));
view_mat = inverse (R) * inverse (T);
glUniformMatrix4fv (view_mat_location, 1, GL_FALSE, view_mat.m);
}
if (GLFW_PRESS == glfwGetKey (g_window, GLFW_KEY_ESCAPE)) {
glfwSetWindowShouldClose (g_window, 1);
}
// put the stuff we've been drawing onto the display
glfwSwapBuffers (g_window);
}
// close GL context and any other GLFW resources
glfwTerminate();
return 0;
}
static void fwdcb(Fl_Widget *, void *) {
fwd();
}
void filter(FILE *f)
{
wint_t c;
int i, w;
while ((c = getwc(f)) != WEOF) switch(c) {
case '\b':
setcol(col - 1);
continue;
case '\t':
setcol((col+8) & ~07);
continue;
case '\r':
setcol(0);
continue;
case SO:
mode |= ALTSET;
continue;
case SI:
mode &= ~ALTSET;
continue;
case IESC:
switch (c = getwc(f)) {
case HREV:
if (halfpos == 0) {
mode |= SUPERSC;
halfpos--;
} else if (halfpos > 0) {
mode &= ~SUBSC;
halfpos--;
} else {
halfpos = 0;
reverse();
}
continue;
case HFWD:
if (halfpos == 0) {
mode |= SUBSC;
halfpos++;
} else if (halfpos < 0) {
mode &= ~SUPERSC;
halfpos++;
} else {
halfpos = 0;
fwd();
}
continue;
case FREV:
reverse();
continue;
default:
errx(EXIT_FAILURE,
_("unknown escape sequence in input: %o, %o"),
IESC, c);
break;
}
continue;
case '_':
if (obuf[col].c_char || obuf[col].c_width < 0) {
while(col > 0 && obuf[col].c_width < 0)
col--;
w = obuf[col].c_width;
for (i = 0; i < w; i++)
obuf[col++].c_mode |= UNDERL | mode;
setcol(col);
continue;
}
obuf[col].c_char = '_';
obuf[col].c_width = 1;
/* fall through */
case ' ':
setcol(col + 1);
continue;
case '\n':
flushln();
continue;
case '\f':
flushln();
putwchar('\f');
continue;
default:
if (!iswprint(c)) /* non printing */
continue;
w = wcwidth(c);
needcol(col + w);
if (obuf[col].c_char == '\0') {
obuf[col].c_char = c;
for (i = 0; i < w; i++)
obuf[col+i].c_mode = mode;
obuf[col].c_width = w;
for (i = 1; i < w; i++)
obuf[col+i].c_width = -1;
} else if (obuf[col].c_char == '_') {
obuf[col].c_char = c;
for (i = 0; i < w; i++)
obuf[col+i].c_mode |= UNDERL|mode;
obuf[col].c_width = w;
for (i = 1; i < w; i++)
obuf[col+i].c_width = -1;
} else if (obuf[col].c_char == c) {
for (i = 0; i < w; i++)
obuf[col+i].c_mode |= BOLD|mode;
} else {
w = obuf[col].c_width;
for (i = 0; i < w; i++)
obuf[col+i].c_mode = mode;
}
setcol(col + w);
continue;
}
if (maxcol)
flushln();
}
bool ProjectedShadow::_updateDecal( const SceneRenderState *state )
{
PROFILE_SCOPE( ProjectedShadow_UpdateDecal );
if ( !LIGHTMGR )
return false;
// Get the position of the decal first.
const Box3F &objBox = mParentObject->getObjBox();
const Point3F boxCenter = objBox.getCenter();
Point3F decalPos = boxCenter;
const MatrixF &renderTransform = mParentObject->getRenderTransform();
{
// Set up the decal position.
// We use the object space box center
// multiplied by the render transform
// of the object to ensure we benefit
// from interpolation.
MatrixF t( renderTransform );
t.setColumn(2,Point3F::UnitZ);
t.mulP( decalPos );
}
if ( mDecalInstance )
{
mDecalInstance->mPosition = decalPos;
if ( !shouldRender( state ) )
return false;
}
// Get the sunlight for the shadow projection.
// We want the LightManager to return NULL if it can't
// get the "real" sun, so we specify false for the useDefault parameter.
LightInfo *lights[4] = {0};
LightQuery query;
query.init( mParentObject->getWorldSphere() );
query.getLights( lights, 4 );
Point3F pos = renderTransform.getPosition();
Point3F lightDir( 0, 0, 0 );
Point3F tmp( 0, 0, 0 );
F32 weight = 0;
F32 range = 0;
U32 lightCount = 0;
F32 dist = 0;
F32 fade = 0;
for ( U32 i = 0; i < 4; i++ )
{
// If we got a NULL light,
// we're at the end of the list.
if ( !lights[i] )
break;
if ( !lights[i]->getCastShadows() )
continue;
if ( lights[i]->getType() != LightInfo::Point )
tmp = lights[i]->getDirection();
else
tmp = pos - lights[i]->getPosition();
range = lights[i]->getRange().x;
dist = ( (tmp.lenSquared()) / ((range * range) * 0.5f));
weight = mClampF( 1.0f - ( tmp.lenSquared() / (range * range)), 0.00001f, 1.0f );
if ( lights[i]->getType() == LightInfo::Vector )
fade = getMax( fade, 1.0f );
else
fade = getMax( fade, mClampF( 1.0f - dist, 0.00001f, 1.0f ) );
lightDir += tmp * weight;
lightCount++;
}
lightDir.normalize();
// No light... no shadow.
if ( !lights[0] )
return false;
// Has the light direction
// changed since last update?
bool lightDirChanged = !mLastLightDir.equal( lightDir );
// Has the parent object moved
// or scaled since the last update?
bool hasMoved = !mLastObjectPosition.equal( mParentObject->getRenderPosition() );
bool hasScaled = !mLastObjectScale.equal( mParentObject->getScale() );
// Set the last light direction
// to the current light direction.
mLastLightDir = lightDir;
mLastObjectPosition = mParentObject->getRenderPosition();
mLastObjectScale = mParentObject->getScale();
// Temps used to generate
// tangent vector for DecalInstance below.
VectorF right( 0, 0, 0 );
VectorF fwd( 0, 0, 0 );
VectorF tmpFwd( 0, 0, 0 );
U32 idx = lightDir.getLeastComponentIndex();
tmpFwd[idx] = 1.0f;
right = mCross( tmpFwd, lightDir );
fwd = mCross( lightDir, right );
right = mCross( fwd, lightDir );
right.normalize();
// Set up the world to light space
// matrix, along with proper position
// and rotation to be used as the world
// matrix for the render to texture later on.
static MatrixF sRotMat(EulerF( 0.0f, -(M_PI_F/2.0f), 0.0f));
mWorldToLight.identity();
MathUtils::getMatrixFromForwardVector( lightDir, &mWorldToLight );
mWorldToLight.setPosition( ( pos + boxCenter ) - ( ( (mRadius * smDepthAdjust) + 0.001f ) * lightDir ) );
mWorldToLight.mul( sRotMat );
mWorldToLight.inverse();
// Get the shapebase datablock if we have one.
ShapeBaseData *data = NULL;
if ( mShapeBase )
data = static_cast<ShapeBaseData*>( mShapeBase->getDataBlock() );
// We use the object box's extents multiplied
// by the object's scale divided by 2 for the radius
// because the object's worldsphere radius is not
// rotationally invariant.
mRadius = (objBox.getExtents() * mParentObject->getScale()).len() * 0.5f;
if ( data )
mRadius *= data->shadowSphereAdjust;
// Create the decal if we don't have one yet.
if ( !mDecalInstance )
mDecalInstance = gDecalManager->addDecal( decalPos,
lightDir,
right,
mDecalData,
1.0f,
0,
PermanentDecal | ClipDecal | CustomDecal );
if ( !mDecalInstance )
return false;
mDecalInstance->mVisibility = fade;
// Setup decal parameters.
mDecalInstance->mSize = mRadius * 2.0f;
mDecalInstance->mNormal = -lightDir;
mDecalInstance->mTangent = -right;
mDecalInstance->mRotAroundNormal = 0;
mDecalInstance->mPosition = decalPos;
mDecalInstance->mDataBlock = mDecalData;
// If the position of the world
// space box center is the same
// as the decal's position, and
// the light direction has not
// changed, we don't need to clip.
bool shouldClip = lightDirChanged || hasMoved || hasScaled;
// Now, check and see if the object is visible.
const Frustum &frust = state->getCullingFrustum();
if ( frust.isCulled( SphereF( mDecalInstance->mPosition, mDecalInstance->mSize * mDecalInstance->mSize ) ) && !shouldClip )
return false;
F32 shadowLen = 10.0f;
if ( data )
shadowLen = data->shadowProjectionDistance;
const Point3F &boxExtents = objBox.getExtents();
mShadowLength = shadowLen * mParentObject->getScale().z;
// Set up clip depth, and box half
// offset for decal clipping.
Point2F clipParams( mShadowLength, (boxExtents.x + boxExtents.y) * 0.25f );
bool render = false;
bool clipSucceeded = true;
// Clip!
if ( shouldClip )
{
clipSucceeded = gDecalManager->clipDecal( mDecalInstance,
NULL,
&clipParams );
}
// If the clip failed,
// we'll return false in
// order to keep from
// unnecessarily rendering
// into the texture. If
// there was no reason to clip
// on this update, we'll assume we
// should update the texture.
render = clipSucceeded;
// Tell the DecalManager we've changed this decal.
gDecalManager->notifyDecalModified( mDecalInstance );
return render;
}
void
filter(FILE *f)
{
wint_t c;
int i, w;
while ((c = getwc(f)) != WEOF && col < MAXBUF) switch(c) {
case '\b':
if (col > 0)
col--;
continue;
case '\t':
col = (col+8) & ~07;
if (col > maxcol)
maxcol = col;
continue;
case '\r':
col = 0;
continue;
case SO:
mode |= ALTSET;
continue;
case SI:
mode &= ~ALTSET;
continue;
case IESC:
switch (c = getwc(f)) {
case HREV:
if (halfpos == 0) {
mode |= SUPERSC;
halfpos--;
} else if (halfpos > 0) {
mode &= ~SUBSC;
halfpos--;
} else {
halfpos = 0;
reverse();
}
continue;
case HFWD:
if (halfpos == 0) {
mode |= SUBSC;
halfpos++;
} else if (halfpos < 0) {
mode &= ~SUPERSC;
halfpos++;
} else {
halfpos = 0;
fwd();
}
continue;
case FREV:
reverse();
continue;
default:
errx(1, "unknown escape sequence in input: %o, %o", IESC, c);
}
continue;
case '_':
if (obuf[col].c_char || obuf[col].c_width < 0) {
while (col > 0 && obuf[col].c_width < 0)
col--;
w = obuf[col].c_width;
for (i = 0; i < w; i++)
obuf[col++].c_mode |= UNDERL | mode;
if (col > maxcol)
maxcol = col;
continue;
}
obuf[col].c_char = '_';
obuf[col].c_width = 1;
/* FALLTHROUGH */
case ' ':
col++;
if (col > maxcol)
maxcol = col;
continue;
case '\n':
flushln();
continue;
case '\f':
flushln();
putwchar('\f');
continue;
default:
if ((w = wcwidth(c)) <= 0) /* non printing */
continue;
if (obuf[col].c_char == '\0') {
obuf[col].c_char = c;
for (i = 0; i < w; i++)
obuf[col + i].c_mode = mode;
obuf[col].c_width = w;
for (i = 1; i < w; i++)
obuf[col + i].c_width = -1;
} else if (obuf[col].c_char == '_') {
obuf[col].c_char = c;
for (i = 0; i < w; i++)
obuf[col + i].c_mode |= UNDERL|mode;
obuf[col].c_width = w;
for (i = 1; i < w; i++)
obuf[col + i].c_width = -1;
} else if (obuf[col].c_char == c) {
for (i = 0; i < w; i++)
obuf[col + i].c_mode |= BOLD|mode;
} else {
w = obuf[col].c_width;
for (i = 0; i < w; i++)
obuf[col + i].c_mode = mode;
}
col += w;
if (col > maxcol)
maxcol = col;
continue;
}
if (ferror(f))
err(1, NULL);
if (maxcol)
flushln();
}
void CHLSL_Mesh::CreatePlane( float size, int subdiv )
{
delete [] m_Vertices;
delete [] m_Triangles;
float dist_per_vert = size / subdiv * 2;
int num_verts_side = ( subdiv + 1 );
m_iNumVertices = num_verts_side * num_verts_side;
int num_tris_side = subdiv;
m_iNumTriangles = num_tris_side * num_tris_side * 2;
m_Vertices = new CHLSL_Vertex[ m_iNumVertices ];
m_Triangles = new CHLSL_Triangle[ m_iNumTriangles ];
Vector fwd( 0, 0, 1 );
Vector right( 0, -1, 0 );
Vector up( -1, 0, 0 );
for ( int vert_right = 0; vert_right < num_verts_side; vert_right++ )
{
for ( int vert_up = 0; vert_up < num_verts_side; vert_up++ )
{
int vertindex = vert_up * num_verts_side +
vert_right;
CHLSL_Vertex &vert = m_Vertices[ vertindex ];
Vector pos = right * -size + up * -size;
pos += right * vert_right * dist_per_vert;
pos += up * vert_up * dist_per_vert;
Vector2D uv( vert_right / (float)(num_verts_side - 1),
vert_up / (float)(num_verts_side - 1) );
Vector n = fwd;
Q_memcpy( vert.normal, n.Base(), sizeof( float ) * 3 );
Q_memcpy( vert.pos, pos.Base(), sizeof( float ) * 3 );
Q_memcpy( vert.uv, uv.Base(), sizeof( float ) * 2 );
}
}
for ( int tri_right = 0; tri_right < num_tris_side; tri_right++ )
{
for ( int tri_up = 0; tri_up < num_tris_side; tri_up++ )
{
int tri_index_0 = tri_up * num_tris_side +
tri_right;
int tri_index_1 = tri_index_0 + m_iNumTriangles / 2;
Assert( tri_index_0 < m_iNumTriangles );
Assert( tri_index_1 < m_iNumTriangles );
int v00_index = tri_up * num_verts_side +
tri_right;
int v10_index = v00_index + 1;
int v01_index = (tri_up+1) * num_verts_side +
tri_right;
int v11_index = v01_index + 1;
Assert( v00_index < m_iNumVertices );
Assert( v01_index < m_iNumVertices );
Assert( v10_index < m_iNumVertices );
Assert( v11_index < m_iNumVertices );
CHLSL_Triangle &t0 = m_Triangles[ tri_index_0 ];
CHLSL_Triangle &t1 = m_Triangles[ tri_index_1 ];
t0.vertices[ 0 ] = &m_Vertices[ v00_index ];
t0.vertices[ 1 ] = &m_Vertices[ v10_index ];
t0.vertices[ 2 ] = &m_Vertices[ v01_index ];
t1.vertices[ 0 ] = &m_Vertices[ v10_index ];
t1.vertices[ 1 ] = &m_Vertices[ v11_index ];
t1.vertices[ 2 ] = &m_Vertices[ v01_index ];
}
}
GenerateTangentSpace();
}
void FlushGalley(OBJECT hd)
{ OBJECT dest; /* the target galley hd empties into */
OBJECT dest_index; /* the index of dest */
OBJECT inners; /* list of galleys and PRECEDES to flush */
OBJECT link, y; /* for scanning through the components of hd */
int dim; /* direction of galley */
CONSTRAINT dest_par_constr; /* the parallel size constraint on dest */
CONSTRAINT dest_perp_constr; /* the perpendicular size constraint on dest */
int pb, pf, f; /* candidate replacement sizes for dest */
OBJECT dest_encl; /* the VCAT or ACAT enclosing dest, if any */
int dest_side; /* if dest_encl != nilobj, side dest is on */
BOOLEAN need_adjust; /* TRUE as soon as dest_encl needs adjusting */
FULL_LENGTH dest_back, dest_fwd; /* the current size of dest_encl or dest */
FULL_LENGTH frame_size; /* the total constraint of dest_encl */
OBJECT prec_gap; /* the gap preceding dest if any else nilobj */
OBJECT prec_def; /* the component preceding dest, if any */
OBJECT succ_gap; /* the gap following dest if any else nilobj */
OBJECT succ_def; /* the component following dest, if any */
OBJECT stop_link; /* most recently seen gap link of hd */
FULL_LENGTH stop_back; /* back(dest_encl) incl all before stop_link */
FULL_LENGTH stop_fwd; /* fwd(dest_encl) incl. all before stop_link */
FULL_LENGTH stop_perp_back; /* back(dest_encl) in other direction */
FULL_LENGTH stop_perp_fwd; /* fwd(dest_encl) in other direction */
BOOLEAN prnt_flush; /* TRUE when the parent of hd needs a flush */
BOOLEAN target_is_internal; /* TRUE if flushing into an internal target */
BOOLEAN headers_seen; /* TRUE if a header is seen at all */
OBJECT zlink, z, tmp, prnt; int attach_status; BOOLEAN remove_target;
OBJECT why;
FULL_LENGTH perp_back, perp_fwd; /* current perp size of dest_encl */
debug1(DGF, D, "[ FlushGalley %s (hd)", SymName(actual(hd)));
prnt_flush = FALSE;
dim = gall_dir(hd);
RESUME:
assert( type(hd) == HEAD, "FlushGalley: type(hd) != HEAD!" );
debug1(DGF, D, " resuming FlushGalley %s, hd =", SymName(actual(hd)));
ifdebugcond(DGF, DD, actual(hd) == nilobj, DebugGalley(hd, nilobj, 4));
assert( Up(hd) != hd, "FlushGalley: resume found no parent to hd!" );
/*@@************************************************************************/
/* */
/* The first step is to examine the parent of galley hd to determine the */
/* status of the galley. If this is not suitable for flushing, we do */
/* what we can to change the status. If still no good, return; so if */
/* this code does not return, then the galley is ready to flush into a */
/* destination in the normal way, and the following variables are set: */
/* */
/* dest_index the parent of the galley and index of its destination */
/* dest the destination of the galley, a @Galley object */
/* */
/***************************************************************************/
Parent(dest_index, Up(hd));
switch( type(dest_index) )
{
case DEAD:
/* the galley has been killed off while this process was sleeping */
debug1(DGF, D, "] FlushGalley %s returning (DEAD)", SymName(actual(hd)));
return;
case UNATTACHED:
/* the galley is currently not attached to a destination */
attach_status = AttachGalley(hd, &inners, &y);
debug1(DGF, DD, " ex-AttachGalley inners: %s", DebugInnersNames(inners));
Parent(dest_index, Up(hd));
switch( attach_status )
{
case ATTACH_KILLED:
assert(inners==nilobj, "FlushGalley/ATTACH_KILLED: inners!=nilobj!");
debug1(DGF, D, "] FlushGalley %s returning (ATTACH_KILLED)",
SymName(actual(hd)));
debug1(DGF, D, " prnt_flush = %s", bool(prnt_flush));
return;
case ATTACH_INPUT:
ParentFlush(prnt_flush, dest_index, FALSE);
assert(inners==nilobj, "FlushGalley/ATTACH_INPUT: inners!=nilobj!");
debug1(DGF, D, "] FlushGalley %s returning (ATTACH_INPUT)",
SymName(actual(hd)));
return;
case ATTACH_NOTARGET:
ParentFlush(prnt_flush, dest_index, FALSE);
assert(inners==nilobj, "FlushGalley/ATTACH_NOTARG: inners!=nilobj!");
debug1(DGF, D, "] FlushGalley %s returning (ATTACH_NOTARGET)",
SymName(actual(hd)));
return;
case ATTACH_SUSPEND:
/* AttachGalley only returns inners here if they really need to */
/* be flushed; in particular the galley must be unsized before */
if( inners != nilobj )
{
debug0(DGF, DD, " calling FlushInners() from FlushGalley (a)");
FlushInners(inners, nilobj);
goto RESUME;
}
stop_link = nilobj;
goto SUSPEND; /* nb y will be set by AttachGalley in this case */
case ATTACH_NULL:
/* hd will have been linked to the unexpanded target in this case */
remove_target = (actual(actual(dest_index)) == whereto(hd));
if( force_gall(hd) )
{
/* if hd is a forcing galley, close all predecessors */
debug3(DGA, D, " forcing ATTACH_NULL case for %s into %s (%s)",
SymName(actual(hd)), SymName(whereto(hd)),
remove_target ? "remove_target" : "not remove_target");
Parent(prnt, Up(dest_index));
if( !non_blocking(dest_index) && remove_target )
{
/* ***
prnt_flush = TRUE;
*** */
prnt_flush = non_blocking(dest_index) = TRUE;
}
FreeGalley(prnt, Up(dest_index), &inners, Up(dest_index),
whereto(hd));
}
else
{
debug3(DGA, D, " non-force ATTACH_NULL case for %s into %s (%s)",
SymName(actual(hd)), SymName(whereto(hd)),
remove_target ? "remove_target" : "not remove_target");
if( blocked(dest_index) && remove_target ) prnt_flush = TRUE;
}
DetachGalley(hd);
KillGalley(hd, TRUE);
if( inners != nilobj )
{
debug0(DGF, DD, " calling FlushInners() from FlushGalley (b)");
FlushInners(inners, nilobj);
}
else ParentFlush(prnt_flush, dest_index, remove_target);
debug0(DGF, D, "] FlushGalley returning ATTACH_NULL");
return;
case ATTACH_ACCEPT:
/* if hd is a forcing galley, or actual(dest_index) is */
/* @ForceGalley, then close all predecessors */
if( force_gall(hd) || actual(actual(dest_index)) == ForceGalleySym )
{ Parent(prnt, Up(dest_index));
debug1(DGA, D, " forcing ATTACH_ACCEPT case for %s",
SymName(actual(hd)));
/* debug0(DGA, DD, " force: prnt ="); */
/* ifdebug(DGA, DD, DebugObject(prnt)); */
/* debug1(DGA, D," calling FreeGalley from FlushGalley(%s)", */
/* SymName(actual(hd))); */
if( !non_blocking(dest_index) ) prnt_flush = TRUE; /* bug fix */
FreeGalley(prnt, Up(dest_index), &inners, Up(dest_index),
whereto(hd));
/* debug0(DGA, DD, " force: after FreeGalley, prnt ="); */
/* ifdebug(DGA, DD, DebugObject(prnt)); */
}
else prnt_flush = prnt_flush || blocked(dest_index);
debug1(DGF, DD, " force: prnt_flush = %s", bool(prnt_flush));
if( inners != nilobj )
{
debug0(DGF, DD, " calling FlushInners() from FlushGalley (c)");
FlushInners(inners, nilobj);
}
goto RESUME;
default:
assert(FALSE, "FlushGalley: attach_status");
break;
}
break;
case RECEIVING:
if( actual(actual(dest_index)) == InputSym )
{ ParentFlush(prnt_flush, dest_index, FALSE);
debug1(DGF, D, "] FlushGalley %s retn, input", SymName(actual(hd)));
return;
}
break;
default:
assert1(FALSE, "FlushGalley: dest_index", Image(type(dest_index)));
break;
}
dest = actual(dest_index);
if( underline(dest) == UNDER_UNDEF ) underline(dest) = UNDER_OFF;
target_is_internal =
(dim==ROWM && !external_ver(dest)) || (dim==COLM && !external_hor(dest));
headers_seen = FALSE;
debug1(DGF, DD, " dest_index: %s", EchoObject(dest_index));
/*@@************************************************************************/
/* */
/* The second step is to examine the components of the galley one by one */
/* to determine if they can be promoted. Each component has the format */
/* */
/* { <index> } <object> */
/* */
/* and is always followed by a gap object (except the last component). */
/* An index indicates that the following object has some interesting */
/* feature, and it points to that feature inside the object. There are */
/* two possible actions for each component, in addition to accepting it: */
/* */
/* REJECT: The component does not fit, so detach the galley */
/* SUSPEND: The component is incomplete; go to sleep and wait */
/* */
/***************************************************************************/
stop_link = dest_encl = inners = nilobj;
need_adjust = FALSE;
/***************************************************************************/
/* */
/* Loop invariant */
/* */
/* The children of hd up to but not including Child(link) have been */
/* examined and pronounced to be promotable, if unbreakable gaps are */
/* ignored. When unbreakable gaps are taken into account, the most */
/* recent gap where a break is possible is at Child(stop_link), or */
/* nowhere if stop_link == nilobj. */
/* */
/* Case 1: target_is_internal == FALSE */
/* */
/* If this flag is FALSE, it means that the target of this galley is */
/* external. Consequently, there is no need to calculate sizes because */
/* there is no constraint on them. Also, a REJECT action is impossible */
/* so unbreakable gaps are no impediment. Variable dest_encl is nilobj. */
/* */
/* Case 2: target_is_internal == TRUE */
/* */
/* If this flag is TRUE, it means that the target of this galley is */
/* internal. Consequently, sizes need to be calculated, and unbreakable */
/* gaps need to be taken into account. Variable dest_encl may be not */
/* nilobj, in which case the following variables are defined: */
/* */
/* dest_encl the object enclosing dest (which must exist) */
/* prec_gap gap object preceding dest (which must exist) */
/* prec_def first definite object preceding dest (must exist) */
/* dest_back back(dest_encl) including effect of accepted compts */
/* dest_fwd fwd(dest_encl) including effect of accepted compts */
/* dest_side BACK or FWD, i.e. which side of the mark dest is on */
/* dest_par_constr the parallel size constraint on dest */
/* dest_perp_constr the perpendicular size constraint on dest */
/* frame_size size of frame enclosing dest_encl */
/* perp_back back(dest_encl) in other direction, incl accepteds */
/* perp_fwd fwd(dest_encl) in other direction, incl accepteds */
/* */
/* if dest_encl is nilobj, these variables are not defined. */
/* */
/* If stop_link is non-nilobj, then in the internal case dest_encl must */
/* be non-nilobj, and the following variables are defined: */
/* */
/* stop_back back(dest_encl) including all before stop_link */
/* stop_fwd fwd(dest_encl) including all before stop_link */
/* stop_perp_back back(dest_encl) in other direction */
/* stop_perp_fwd fwd(dest_encl) in other direction */
/* */
/* need_adjust is true if at least one definite component has been */
/* accepted for promotion and the destination is internal; hence, */
/* dest_encl is defined and its size needs to be adjusted. */
/* */
/* inners is the set of all PRECEDES and UNATTACHED indexes found. */
/* */
/***************************************************************************/
for( link = Down(hd); link != hd; link = NextDown(link) )
{
Child(y, link);
if( type(y) == SPLIT ) Child(y, DownDim(y, dim));
debug2(DGF, DD, " examining %s %s", Image(type(y)), EchoObject(y));
switch( type(y) )
{
case GAP_OBJ:
underline(y) = underline(dest);
prec_gap = y;
if( target_is_internal )
{
/* *** not necessarily true
assert( dest_encl != nilobj, "FlushGalley/GAP_OBJ: dest_encl!" );
*** */
if( dest_encl != nilobj && !nobreak(gap(prec_gap)) )
{
stop_link = link;
stop_back = dest_back;
stop_fwd = dest_fwd;
stop_perp_back = perp_back;
stop_perp_fwd = perp_fwd;
}
}
else stop_link = link;
if( !join(gap(y)) ) seen_nojoin(hd) = TRUE;
break;
case SCALE_IND:
case COVER_IND:
case EXPAND_IND:
case GALL_PREC:
case GALL_FOLL:
case GALL_FOLL_OR_PREC:
case GALL_TARG:
case CROSS_PREC:
case CROSS_FOLL:
case CROSS_FOLL_OR_PREC:
case CROSS_TARG:
case PAGE_LABEL_IND:
underline(y) = underline(dest);
break;
case PRECEDES:
case UNATTACHED:
if( inners == nilobj ) New(inners, ACAT);
Link(inners, y);
break;
case RECEIVING:
case RECEPTIVE:
goto SUSPEND;
case FOLLOWS:
Child(tmp, Down(y));
if( Up(tmp) == LastUp(tmp) )
{ link = PrevDown(link);
DisposeChild(NextDown(link));
break;
}
Parent(tmp, Up(tmp));
assert(type(tmp) == PRECEDES, "Flush: PRECEDES!");
switch( CheckComponentOrder(tmp, dest_index) )
{
case CLEAR: DeleteNode(tmp);
link = PrevDown(link);
DisposeChild(NextDown(link));
break;
case PROMOTE: break;
case BLOCK: goto SUSPEND;
case CLOSE: if( opt_components(hd) != nilobj )
{ DisposeObject(opt_components(hd));
opt_components(hd) = nilobj;
debug2(DOG, D, "FlushGalley(%s) de-optimizing %s",
"(CLOSE problem)", SymName(actual(hd)));
}
debug1(DGF, DD, " reject (a) %s", EchoObject(y));
goto REJECT;
}
break;
case BEGIN_HEADER:
case END_HEADER:
case SET_HEADER:
case CLEAR_HEADER:
/* do nothing except take note, until actually promoted out of here */
headers_seen = TRUE;
break;
case NULL_CLOS:
case PAGE_LABEL:
case WORD:
case QWORD:
case ONE_COL:
case ONE_ROW:
case WIDE:
case HIGH:
case HSHIFT:
case VSHIFT:
case HSCALE:
case VSCALE:
case HCOVER:
case VCOVER:
case HCONTRACT:
case VCONTRACT:
case HLIMITED:
case VLIMITED:
case HEXPAND:
case VEXPAND:
case START_HVSPAN:
case START_HSPAN:
case START_VSPAN:
case HSPAN:
case VSPAN:
case ROTATE:
case BACKGROUND:
case SCALE:
case KERN_SHRINK:
case INCGRAPHIC:
case SINCGRAPHIC:
case PLAIN_GRAPHIC:
case GRAPHIC:
case LINK_SOURCE:
case LINK_DEST:
case ACAT:
case HCAT:
case VCAT:
case ROW_THR:
case CLOSURE:
case CROSS:
case FORCE_CROSS:
underline(y) = underline(dest);
if( dim == ROWM )
{
/* make sure y is not joined to a target below (vertical case only) */
for( zlink = NextDown(link); zlink != hd; zlink = NextDown(zlink) )
{ Child(z, zlink);
switch( type(z) )
{
case RECEPTIVE:
case RECEIVING: y = z;
goto SUSPEND;
case GAP_OBJ: if( !join(gap(z)) ) zlink = PrevDown(hd);
break;
default: break;
}
}
/* try vertical hyphenation before anything else */
if( type(y) == HCAT ) VerticalHyphenate(y);
}
/* check size constraint */
if( target_is_internal )
{
/* initialise dest_encl etc if not done yet */
if( dest_encl == nilobj )
{ assert( UpDim(dest,1-dim) == UpDim(dest,dim), "FlushG: UpDims!" );
/* *** weird old code, trying for UpDim(dest, ROWM)?
Parent(dest_encl, NextDown(Up(dest)));
*** */
Parent(dest_encl, Up(dest));
debug4(DGF, DD, " flush dest = %s %s, dest_encl = %s %s",
Image(type(dest)), EchoObject(dest),
Image(type(dest_encl)), EchoObject(dest_encl));
assert( (dim==ROWM && type(dest_encl)==VCAT) ||
(dim==COLM && type(dest_encl)==ACAT),
"FlushGalley: dest != VCAT or ACAT!" );
SetNeighbours(Up(dest), FALSE, &prec_gap, &prec_def,
&succ_gap, &succ_def, &dest_side);
assert(prec_gap != nilobj || is_indefinite(type(y)),
"FlushGalley: prec_gap == nilobj && !is_indefinite(type(y))!" );
assert(succ_gap == nilobj, "FlushGalley: succ_gap != nilobj!" );
assert(dest_side == FWD || is_indefinite(type(y)),
"FlushGalley: dest_side != FWD || !is_indefinite(type(y))!");
dest_back = back(dest_encl, dim);
dest_fwd = fwd(dest_encl, dim);
perp_back = back(dest_encl, 1-dim);
perp_fwd = fwd(dest_encl, 1-dim);
Constrained(dest_encl, &dest_par_constr, dim, &why);
Constrained(dest_encl, &dest_perp_constr, 1-dim, &why);
debug1(DGF, DD, " setting dest_perp_constr = %s",
EchoConstraint(&dest_perp_constr));
frame_size = constrained(dest_par_constr) ? bfc(dest_par_constr) :0;
}
if( !is_indefinite(type(y)) )
{
ifdebugcond(DGF, DD, mode(gap(prec_gap)) == NO_MODE,
DebugGalley(hd, y, 4));
/* calculate parallel effect of adding y to dest */
f = dest_fwd + fwd(y, dim) - fwd(prec_def, dim) +
ActualGap(fwd(prec_def, dim), back(y, dim),
fwd(y, dim), &gap(prec_gap), frame_size,
dest_back + dest_fwd - fwd(prec_def, dim));
debug5(DGF, DD, " f = %s + %s - %s + %s (prec_gap %s)",
EchoLength(dest_fwd), EchoLength(fwd(y, dim)),
EchoLength(fwd(prec_def, dim)), EchoLength(
ActualGap(fwd(prec_def, dim), back(y, dim),
fwd(y, dim), &gap(prec_gap), frame_size,
dest_back + dest_fwd - fwd(prec_def, dim))
), EchoGap(&gap(prec_gap)));
debug3(DGF, DD, " b,f: %s,%s; dest_encl: %s",
EchoLength(dest_back), EchoLength(f),
EchoConstraint(&dest_par_constr));
/* check new size against parallel constraint */
if( (units(gap(prec_gap))==FRAME_UNIT && width(gap(prec_gap)) > FR)
|| !FitsConstraint(dest_back, f, dest_par_constr)
|| (opt_components(hd) != nilobj && opt_comps_permitted(hd)<=0)
)
{
if( opt_components(hd) != nilobj )
{ OBJECT z;
/* record the size of this just-completed target area for hd */
New(z, WIDE);
CopyConstraint(constraint(z), dest_par_constr);
Link(opt_constraints(hd), z);
ifdebug(DOG, D,
debug2(DOG, D, "FlushGalley(%s) adding constraint %s",
SymName(actual(hd)), EchoConstraint(&constraint(z)));
if( units(gap(prec_gap))==FRAME_UNIT &&
width(gap(prec_gap)) > FR )
{ debug1(DOG, D, " prec_gap = %s", EchoGap(&gap(prec_gap)));
}
if( !FitsConstraint(dest_back, f, dest_par_constr) )
{ debug3(DOG, D, " !FitsConstraint(%s, %s, %s)",
EchoLength(dest_back), EchoLength(f),
EchoConstraint(&dest_par_constr));
}
if( opt_comps_permitted(hd) <= 0 )
{ debug1(DOG, D, " opt_comps_permitted = %2d",
opt_comps_permitted(hd));
}
debug4(DOG, D, "prec_gap = %s; y = %s (%s,%s):",
EchoGap(&gap(prec_gap)), Image(type(y)),
EchoLength(back(y, dim)), EchoLength(fwd(y, dim)));
DebugObject(y);
)
/* refresh the number of components permitted into the next target */
if( opt_counts(hd) != nilobj && Down(opt_counts(hd)) != opt_counts(hd) )
{ Child(z, Down(opt_counts(hd)));
opt_comps_permitted(hd) += comp_count(z) - 1;
DisposeChild(Up(z));
}
else opt_comps_permitted(hd) = MAX_FILES; /* a large number */
debug1(DOG, D, " REJECT permitted = %2d", opt_comps_permitted(hd));
}
debug1(DGF, DD, " reject (b) %s", EchoObject(y));
goto REJECT;
}
/* calculate perpendicular effect of adding y to dest */
if( seen_nojoin(hd) )
{
pb = 0;
pf = find_max(perp_fwd, size(y, 1-dim));
}
else
{
pb = find_max(perp_back, back(y, 1-dim));
pf = find_max(perp_fwd, fwd(y, 1-dim));
}
/* check new size against perpendicular constraint */
if( !FitsConstraint(pb, pf, dest_perp_constr) )
{
if( opt_components(hd) != nilobj )
{ DisposeObject(opt_components(hd));
opt_components(hd) = nilobj;
debug1(DOG, D, "FlushGalley(%s) de-optimizing (perp problem)",
SymName(actual(hd)));
}
if( dim == ROWM )
{
Error(20, 3, "component too wide for available space",
WARN, &fpos(y));
debug6(DGF, DD, " %s,%s [%s,%s] too wide for %s, y = %s",
EchoLength(pb), EchoLength(pf),
EchoLength(back(y, 1-dim)), EchoLength(fwd(y, 1-dim)),
EchoConstraint(&dest_perp_constr), EchoObject(y));
}
debug1(DGF, DD, " reject (c) %s", EchoObject(y));
goto REJECT;
}
/* accept definite component */
dest_fwd = f; prec_def = y;
perp_back = pb; perp_fwd = pf;
need_adjust = TRUE;
if( opt_components(hd) != nilobj )
{ opt_comps_permitted(hd)--;
debug1(DOG, D, " ACCEPT permitted = %2d", opt_comps_permitted(hd));
}
}
/* accept indefinite component */
} /* end if( target_is_internal ) */
static void
filtr(struct iblok *f)
{
wint_t c;
char b, *cp;
int i, n, w = 0;
while((mb_cur_max > 1 ? cp = ib_getw(f, &c, &n) :
(b = c = ib_get(f), n = 1,
cp = c == (wint_t)EOF ? 0 : &b)),
cp != NULL) switch(c) {
case '\b':
if (col > 0)
col--;
continue;
case '\t':
col = (col+8) & ~07;
if (col > maxcol)
maxcol = col;
continue;
case '\r':
col = 0;
continue;
case SO:
mode |= ALTSET;
continue;
case SI:
mode &= ~ALTSET;
continue;
case IESC:
mb_cur_max > 1 ? cp = ib_getw(f, &c, &n) :
(b = c = ib_get(f), n = 1,
cp = c == (wint_t)EOF ? 0 : &b);
switch (c) {
case HREV:
if (halfpos == 0) {
mode |= SUPERSC;
halfpos--;
} else if (halfpos > 0) {
mode &= ~SUBSC;
halfpos--;
} else {
halfpos = 0;
reverse();
}
continue;
case HFWD:
if (halfpos == 0) {
mode |= SUBSC;
halfpos++;
} else if (halfpos < 0) {
mode &= ~SUPERSC;
halfpos++;
} else {
halfpos = 0;
fwd();
}
continue;
case FREV:
reverse();
continue;
default:
fprintf(stderr,
"Unknown escape sequence in input: %o, %o\n",
IESC, cp?*cp:EOF);
exit(1);
}
continue;
case '_':
obaccs(col);
if (obuf[col].c_char)
obuf[col].c_mode |= UNDERL | mode;
else
obuf[col].c_char = '_';
case ' ':
col++;
if (col > maxcol)
maxcol = col;
continue;
case '\n':
flushln();
continue;
default:
if (mb_cur_max > 1 ? c == WEOF || (w = wcwidth(c)) < 0 ||
w == 0 && !iswprint(c)
: (w = 1, c == (wint_t)EOF || !isprint(c)))
/* non printing */
continue;
obaccs(col);
if (obuf[col].c_char == '\0') {
obuf[col].c_char = c;
obuf[col].c_mode = mode;
} else if (obuf[col].c_char == '_') {
obuf[col].c_char = c;
obuf[col].c_mode |= UNDERL|mode;
} else if (obuf[col].c_char == c)
obuf[col].c_mode |= BOLD|mode;
else {
obuf[col].c_mode = c;
obuf[col].c_mode = mode;
}
obaccs(col+w-1);
for (i = 1; i < w; i++) {
obuf[col+i].c_mode = obuf[col].c_mode|FILLER;
obuf[col+i].c_char = obuf[col].c_char;
}
col += w;
if (col > maxcol)
maxcol = col;
continue;
}
if (maxcol)
flushln();
}
// ----------------------------------------------------------------------------
void btKart::updateVehicle( btScalar step )
{
for (int i=0;i<getNumWheels();i++)
{
updateWheelTransform(i,false);
}
const btTransform& chassisTrans = getChassisWorldTransform();
btVector3 forwardW(chassisTrans.getBasis()[0][m_indexForwardAxis],
chassisTrans.getBasis()[1][m_indexForwardAxis],
chassisTrans.getBasis()[2][m_indexForwardAxis]);
// Simulate suspension
// -------------------
m_num_wheels_on_ground = 0;
m_visual_wheels_touch_ground = true;
for (int i=0;i<m_wheelInfo.size();i++)
{
btScalar depth;
depth = rayCast( i);
if(m_wheelInfo[i].m_raycastInfo.m_isInContact)
m_num_wheels_on_ground++;
}
// If the kart is flying, try to keep it parallel to the ground.
if(m_num_wheels_on_ground==0)
{
btVector3 kart_up = getChassisWorldTransform().getBasis().getColumn(1);
btVector3 terrain_up(0,1,0);
btVector3 axis = kart_up.cross(terrain_up);
// Give a nicely balanced feeling for rebalancing the kart
m_chassisBody->applyTorqueImpulse(axis * m_kart->getKartProperties()->getSmoothFlyingImpulse());
}
// Work around: make sure that either both wheels on one axis
// are on ground, or none of them. This avoids the problem of
// the kart suddenly getting additional angular velocity because
// e.g. only one rear wheel is on the ground.
for(int i=0; i<m_wheelInfo.size(); i+=2)
{
if( m_wheelInfo[i ].m_raycastInfo.m_isInContact !=
m_wheelInfo[i+1].m_raycastInfo.m_isInContact)
{
int wheel_air_index = i;
int wheel_ground_index = i+1;
if (m_wheelInfo[i].m_raycastInfo.m_isInContact)
{
wheel_air_index = i+1;
wheel_ground_index = i;
}
btWheelInfo& wheel_air = m_wheelInfo[wheel_air_index];
btWheelInfo& wheel_ground = m_wheelInfo[wheel_ground_index];
wheel_air.m_raycastInfo = wheel_ground.m_raycastInfo;
}
} // for i=0; i<m_wheelInfo.size(); i+=2
updateSuspension(step);
for (int i=0;i<m_wheelInfo.size();i++)
{
//apply suspension force
btWheelInfo& wheel = m_wheelInfo[i];
btScalar suspensionForce = wheel.m_wheelsSuspensionForce;
if (suspensionForce > wheel.m_maxSuspensionForce)
{
suspensionForce = wheel.m_maxSuspensionForce;
}
btVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS
* suspensionForce * step;
btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS
- getRigidBody()->getCenterOfMassPosition();
getRigidBody()->applyImpulse(impulse, relpos);
}
updateFriction( step);
for (int i=0;i<m_wheelInfo.size();i++)
{
btWheelInfo& wheel = m_wheelInfo[i];
btVector3 relpos = wheel.m_raycastInfo.m_hardPointWS
- getRigidBody()->getCenterOfMassPosition();
btVector3 vel = getRigidBody()->getVelocityInLocalPoint(relpos);
if (wheel.m_raycastInfo.m_isInContact)
{
const btTransform& chassisWorldTransform =
getChassisWorldTransform();
btVector3 fwd (
chassisWorldTransform.getBasis()[0][m_indexForwardAxis],
chassisWorldTransform.getBasis()[1][m_indexForwardAxis],
chassisWorldTransform.getBasis()[2][m_indexForwardAxis]);
btScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS);
fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj;
btScalar proj2 = fwd.dot(vel);
wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelsRadius);
wheel.m_rotation += wheel.m_deltaRotation;
} else
{
wheel.m_rotation += wheel.m_deltaRotation;
}
//damping of rotation when not in contact
wheel.m_deltaRotation *= btScalar(0.99);
}
float f = -m_kart->getSpeed()
* m_kart->getKartProperties()->getDownwardImpulseFactor();
btVector3 downwards_impulse = m_chassisBody->getWorldTransform().getBasis()
* btVector3(0, f, 0);
m_chassisBody->applyCentralImpulse(downwards_impulse);
if(m_time_additional_impulse>0)
{
float dt = step > m_time_additional_impulse
? m_time_additional_impulse
: step;
m_chassisBody->applyCentralImpulse(m_additional_impulse*dt);
m_time_additional_impulse -= dt;
}
if(m_time_additional_rotation>0)
{
btTransform &t = m_chassisBody->getWorldTransform();
float dt = step > m_time_additional_rotation
? m_time_additional_rotation
: step;
btQuaternion add_rot(m_additional_rotation.getY()*dt,
m_additional_rotation.getX()*dt,
m_additional_rotation.getZ()*dt);
t.setRotation(t.getRotation()*add_rot);
m_chassisBody->setWorldTransform(t);
// Also apply the rotation to the interpolated world transform.
// This is important (at least if the rotation is only applied
// in one frame) since STK will actually use the interpolated
// transform, which would otherwise only be updated one frame
// later, resulting in a one-frame incorrect rotation of the
// kart, or a strongly 'visual jolt' of the kart
btTransform &iwt=m_chassisBody->getInterpolationWorldTransform();
iwt.setRotation(iwt.getRotation()*add_rot);
m_time_additional_rotation -= dt;
}
} // updateVehicle
void ResumeMovieView::CreateMenu( App * app, OvrVRMenuMgr & menuMgr, BitmapFont const & font )
{
Menu = VRMenu::Create( "ResumeMoviePrompt" );
Vector3f fwd( 0.0f, 0.0f, 1.0f );
Vector3f up( 0.0f, 1.0f, 0.0f );
Vector3f defaultScale( 1.0f );
Array< VRMenuObjectParms const * > parms;
VRMenuFontParms fontParms( true, true, false, false, false, 1.3f );
Quatf orientation( Vector3f( 0.0f, 1.0f, 0.0f ), 0.0f );
Vector3f centerPos( 0.0f, 0.0f, 0.0f );
VRMenuObjectParms centerRootParms( VRMENU_CONTAINER, Array< VRMenuComponent* >(), VRMenuSurfaceParms(), "CenterRoot",
Posef( orientation, centerPos ), Vector3f( 1.0f, 1.0f, 1.0f ), fontParms,
ID_CENTER_ROOT, VRMenuObjectFlags_t(), VRMenuObjectInitFlags_t( VRMENUOBJECT_INIT_FORCE_POSITION ) );
parms.PushBack( ¢erRootParms );
Menu->InitWithItems( menuMgr, font, 0.0f, VRMenuFlags_t(), parms );
parms.Clear();
// the centerroot item will get touch relative and touch absolute events and use them to rotate the centerRoot
menuHandle_t centerRootHandle = Menu->HandleForId( menuMgr, ID_CENTER_ROOT );
VRMenuObject * centerRoot = menuMgr.ToObject( centerRootHandle );
OVR_ASSERT( centerRoot != NULL );
// ==============================================================================
//
// title
//
{
Posef panelPose( Quatf( up, 0.0f ), Vector3f( 0.0f, 2.2f, -3.0f ) );
VRMenuObjectParms p( VRMENU_STATIC, Array< VRMenuComponent* >(),
VRMenuSurfaceParms(), Strings::ResumeMenu_Title, panelPose, defaultScale, fontParms, VRMenuId_t( ID_TITLE.Get() ),
VRMenuObjectFlags_t(), VRMenuObjectInitFlags_t( VRMENUOBJECT_INIT_FORCE_POSITION ) );
parms.PushBack( &p );
Menu->AddItems( menuMgr, font, parms, centerRootHandle, false );
parms.Clear();
}
// ==============================================================================
//
// options
//
Array<const char *> options;
options.PushBack( Strings::ResumeMenu_Resume );
options.PushBack( Strings::ResumeMenu_Restart );
Array<const char *> icons;
icons.PushBack( "assets/resume.png" );
icons.PushBack( "assets/restart.png" );
Array<PanelPose> optionPositions;
optionPositions.PushBack( PanelPose( Quatf( up, 0.0f / 180.0f * Mathf::Pi ), Vector3f( -0.5f, 1.7f, -3.0f ), Vector4f( 1.0f, 1.0f, 1.0f, 1.0f ) ) );
optionPositions.PushBack( PanelPose( Quatf( up, 0.0f / 180.0f * Mathf::Pi ), Vector3f( 0.5f, 1.7f, -3.0f ), Vector4f( 1.0f, 1.0f, 1.0f, 1.0f ) ) );
int borderWidth = 0, borderHeight = 0;
GLuint borderTexture = LoadTextureFromApplicationPackage( "assets/resume_restart_border.png", TextureFlags_t( TEXTUREFLAG_NO_DEFAULT ), borderWidth, borderHeight );
for ( int i = 0; i < optionPositions.GetSizeI(); ++i )
{
ResumeMovieComponent * resumeMovieComponent = new ResumeMovieComponent( this, i );
Array< VRMenuComponent* > optionComps;
optionComps.PushBack( resumeMovieComponent );
VRMenuSurfaceParms panelSurfParms( "",
borderTexture, borderWidth, borderHeight, SURFACE_TEXTURE_ADDITIVE,
0, 0, 0, SURFACE_TEXTURE_MAX,
0, 0, 0, SURFACE_TEXTURE_MAX );
Posef panelPose( optionPositions[ i ].Orientation, optionPositions[ i ].Position );
VRMenuObjectParms * p = new VRMenuObjectParms( VRMENU_BUTTON, optionComps,
panelSurfParms, options[ i ], panelPose, defaultScale, fontParms, VRMenuId_t( ID_OPTIONS.Get() + i ),
VRMenuObjectFlags_t(), VRMenuObjectInitFlags_t( VRMENUOBJECT_INIT_FORCE_POSITION ) );
parms.PushBack( p );
Menu->AddItems( menuMgr, font, parms, centerRootHandle, false );
DeletePointerArray( parms );
parms.Clear();
// add icon
menuHandle_t optionHandle = centerRoot->ChildHandleForId( menuMgr, VRMenuId_t( ID_OPTIONS.Get() + i ) );
VRMenuObject * optionObject = menuMgr.ToObject( optionHandle );
OVR_ASSERT( optionObject != NULL );
int iconWidth = 0, iconHeight = 0;
GLuint iconTexture = LoadTextureFromApplicationPackage( icons[ i ], TextureFlags_t( TEXTUREFLAG_NO_DEFAULT ), iconWidth, iconHeight );
VRMenuSurfaceParms iconSurfParms( "",
iconTexture, iconWidth, iconHeight, SURFACE_TEXTURE_DIFFUSE,
0, 0, 0, SURFACE_TEXTURE_MAX,
0, 0, 0, SURFACE_TEXTURE_MAX );
Bounds3f textBounds = optionObject->GetTextLocalBounds( font );
optionObject->SetTextLocalPosition( Vector3f( iconWidth * VRMenuObject::DEFAULT_TEXEL_SCALE * 0.5f, 0.0f, 0.0f ) );
Posef iconPose( optionPositions[ i ].Orientation, optionPositions[ i ].Position + Vector3f( textBounds.GetMins().x, 0.0f, 0.01f ) );
p = new VRMenuObjectParms( VRMENU_STATIC, Array< VRMenuComponent* >(),
iconSurfParms, NULL, iconPose, defaultScale, fontParms, VRMenuId_t( ID_OPTION_ICONS.Get() + i ),
VRMenuObjectFlags_t( VRMENUOBJECT_DONT_HIT_ALL ), VRMenuObjectInitFlags_t( VRMENUOBJECT_INIT_FORCE_POSITION ) );
parms.PushBack( p );
Menu->AddItems( menuMgr, font, parms, centerRootHandle, false );
DeletePointerArray( parms );
parms.Clear();
menuHandle_t iconHandle = centerRoot->ChildHandleForId( menuMgr, VRMenuId_t( ID_OPTION_ICONS.Get() + i ) );
resumeMovieComponent->Icon = menuMgr.ToObject( iconHandle );
}
Cinema.app->GetGuiSys().AddMenu( Menu );
}
int closure (
const char *from,
const char *to,
double parm,
int (*fwd)(SPX_ARGS),
int (*rev)(SPX_ARGS),
const double spec1[],
double spec2[])
{
static char skip = '\0';
int nFail = 0, stat1[NSPEC], stat2[NSPEC], status;
register int j;
double clos[NSPEC], resid, residmax;
/* Convert the first to the second. */
if ((status = fwd(parm, NSPEC, 1, 1, spec1, spec2, stat1))) {
printf("%s%s ERROR %d: %s.\n", from, to, status, spx_errmsg[status]);
}
/* Convert the second back to the first. */
if ((status = rev(parm, NSPEC, 1, 1, spec2, clos, stat2))) {
printf("%s%s ERROR %d: %s.\n", to, from, status, spx_errmsg[status]);
}
residmax = 0.0;
/* Test closure. */
for (j = 0; j < NSPEC; j++) {
if (stat1[j]) {
printf("%c%s%s: %s = %.12e -> %s = ???, stat = %d\n", skip, from, to,
from, spec1[j], to, stat1[j]);
skip = '\0';
continue;
}
if (stat2[j]) {
printf("%c%s%s: %s = %.12e -> %s = %.12e -> %s = ???, stat = %d\n",
skip, to, from, from, spec1[j], to, spec2[j], from, stat2[j]);
skip = '\0';
continue;
}
if (spec1[j] == 0.0) {
resid = fabs(clos[j] - spec1[j]);
} else {
resid = fabs((clos[j] - spec1[j])/spec1[j]);
if (resid > residmax) residmax = resid;
}
if (resid > tol) {
nFail++;
printf("%c%s%s: %s = %.12e -> %s = %.12e ->\n %s = %.12e, "
"resid = %.1e\n", skip, from, to, from, spec1[j], to,
spec2[j], from, clos[j], resid);
skip = '\0';
}
}
printf("%s%s: Maximum closure residual = %.1e\n", from, to, residmax);
if (residmax > tol) {
printf("\n");
skip = '\0';
} else {
skip = '\n';
}
return nFail;
}
/**
* Create our tasks. This uses the interpreter to make the object.
*
* @param mgr
*/
void CoupleTrackletCar(AliAnalysisManager* mgr)
{
AliAnalysisManager::SetCommonFileName("tracklet_dndeta.root");
TString fwd(gSystem->Getenv("ANA_SRC"));
if (fwd.IsNull()) fwd = "$ALICE_PHYSICS/PWGLF/FORWARD/analysis2";
gROOT->SetMacroPath(Form("%s:%s/dndeta/tracklets3",
gROOT->GetMacroPath(), fwd.Data()));
gSystem->AddIncludePath(Form("-I%s/dndeta/tracklets3", fwd.Data()));
Info("CreateTasks", "Loading code");
fRailway->LoadSource("FixPaths.C");
fRailway->LoadSource("AliAODSimpleHeader.C");
fRailway->LoadSource("AliAODTracklet.C");
fRailway->LoadSource("AliTrackletWeights.C");
fRailway->LoadSource("AliTrackletAODUtils.C");
fRailway->LoadSource("AliTrackletAODdNdeta.C");
// --- Create the task using interpreter -------------------------
Bool_t mc = fOptions.Has("mc");
if (!mc) mc = fRailway->IsMC();
Long_t ret =
gROOT->ProcessLine(Form("AliTrackletAODdNdeta::Create(%d,\"%s\")",mc,
fOptions.AsString("reweigh")));
AliAnalysisTaskSE* task = reinterpret_cast<AliAnalysisTaskSE*>(ret);
if (!task) return;
// --- Figure out the trigger options ----------------------------
TString trg = fOptions.Get("trig"); trg.ToUpper();
UInt_t sel = AliVEvent::kINT7;
UInt_t mb = AliVEvent::kINT1; // AliVEvent::kMB;
if (trg.EqualTo("MB")) sel = mb;
else if (trg.EqualTo("MBOR")) sel = mb;
else if (trg.EqualTo("INEL")) sel = mb;
else if (trg.EqualTo("INELGT0")) sel = mb;
else if (trg.EqualTo("V0AND")) sel = AliVEvent::kINT7;
else if (trg.EqualTo("NSD")) sel = AliVEvent::kINT7;
else if (trg.EqualTo("V0OR")) sel = AliVEvent::kCINT5;
else if (trg.EqualTo("ANY")) sel = AliVEvent::kAny;
task->SelectCollisionCandidates(sel);
Int_t minTrk = trg.EqualTo("INELGT0") ? 1 : 0;
// --- Figure out calculation mode -------------------------------
TString calc = fOptions.Get("reweigh-calc"); calc.ToUpper();
UChar_t mcal = 0;
if (calc.BeginsWith("PROD")) mcal = 0;
else if (calc.BeginsWith("SQ")) mcal = 1;
else if (calc.BeginsWith("SUM")) mcal = 2;
else if (calc.BeginsWith("AV")) mcal = 3;
// --- Set various options on task -------------------------------
const char* defCent = "0-5-10-20-30-40-50-60-70-80-90";
FromOption(task, "CentralityMethod","cent", "V0M");
FromOption(task, "CentralityAxis", "cent-bins", defCent);
FromOption(task, "EtaAxis", "eta-bins", "r16:2");
FromOption(task, "IPzAxis", "ipz-bins", "u15");
FromOption(task, "DeltaCut", "delta-cut", 1.5);
FromOption(task, "TailDelta", "tail-delta", 5.);
FromOption(task, "TailMaximum", "tail-max", -1);
FromOption(task, "MaxDelta", "max-delta", 25.);
FromOption(task, "DPhiShift", "dphi-shift", 0.0045);
FromOption(task, "ShiftedDPhiCut", "shifted-dphi-cut",-1.);
FromOption(task, "AbsMinCent", "abs-min-cent", -1.);
FromOption(task, "WeightMask", "reweigh-mask", 0xFF);
FromOption(task, "WeightVeto", "reweigh-veto", 0x0);
SetOnTask (task, "WeightCalc", mcal);
FromOption(task, "WeightInverse", "reweigh-inv", false);
FromOption(task, "TriggerEff", "trigEff", 0.);
SetOnTask (task, "MinEta1", minTrk);
// if (mc && we) {
// TUrl wurl(fOptions.AsString("reweight"));
// TFile* wfile = TFile::Open(wurl.GetFile());
// if (!wfile) {
// Warning("CreateTasks", "Failed to open weights file: %s",
// wurl.GetUrl());
// return;
// }
// TString wnam(wurl.GetAnchor());
// if (wnam.IsNull()) wnam = "weights";
// TObject* wobj = wfile->Get(wnam);
// if (!wobj) {
// Warning("CreateTasks", "Failed to get weights %s from file %s",
// wnam.Data(), wfile->GetName());
// return;
// }
// if (!wobj->IsA()->InheritsFrom("AliTrackletWeights")) {
// Warning("CreateTasks", "Object %s from file %s not an "
// "AliTrackletWeights but a %s",
// wnam.Data(), wfile->GetName(), wobj->ClassName());
// return;
// }
// SetOnTaskGeneric(task, "Weights",
// Form("((AliTrackletWeights*)%p)", wobj));
// }
Printf("Print the generated task");
task->Print("");
}
void test(Test k) {
Test fwd(std::forward<Test>(k));
// Test fwd(k);
fwd.print();
}
void RaycastCar::update_engine(btScalar step)
{
const btTransform & chassis_t = getChassisWorldTransform();
btVector3 fwd(chassis_t.getBasis()[0][m_indexForwardAxis],
chassis_t.getBasis()[1][m_indexForwardAxis],
chassis_t.getBasis()[2][m_indexForwardAxis]);
WheelInfo & wheelInfo0 = m_wheelInfo[0];
wheelInfo0.m_last_linear_velocity = wheelInfo0.m_linear_velocity;
btVector3 relpos0 = wheelInfo0.m_raycastInfo.m_hardPointWS - getRigidBody()->getCenterOfMassPosition();
btVector3 vel0 = getRigidBody()->getVelocityInLocalPoint(relpos0);
btScalar proj0 = fwd.dot(wheelInfo0.m_raycastInfo.m_contactNormalWS);
btVector3 fwd_w0 = fwd - wheelInfo0.m_raycastInfo.m_contactNormalWS * proj0;
btScalar lin_vel0 = fwd_w0.dot(vel0);
btScalar w0_rpm = fabs(rads2rpm(lin_vel0/wheelInfo0.m_wheelsRadius));
btScalar delta0 = (lin_vel0/wheelInfo0.m_wheelsRadius)*step;
wheelInfo0.m_linear_velocity = lin_vel0;
wheelInfo0.m_angular_velocity = lin_vel0/wheelInfo0.m_wheelsRadius;
WheelInfo & wheelInfo1 = m_wheelInfo[1];
wheelInfo1.m_last_linear_velocity = wheelInfo1.m_linear_velocity;
btVector3 relpos1 = wheelInfo1.m_raycastInfo.m_hardPointWS - getRigidBody()->getCenterOfMassPosition();
btVector3 vel1 = getRigidBody()->getVelocityInLocalPoint(relpos1);
btScalar proj1 = fwd.dot(wheelInfo1.m_raycastInfo.m_contactNormalWS);
btVector3 fwd_w1 = fwd - wheelInfo1.m_raycastInfo.m_contactNormalWS * proj1;
btScalar lin_vel1 = fwd_w1.dot(vel1);
btScalar w1_rpm = fabs(rads2rpm(lin_vel1/wheelInfo1.m_wheelsRadius));
btScalar delta1 = (lin_vel1/wheelInfo1.m_wheelsRadius)*step;
wheelInfo1.m_linear_velocity = lin_vel1;
wheelInfo1.m_angular_velocity = lin_vel1/wheelInfo1.m_wheelsRadius;
WheelInfo & wheelInfo2 = m_wheelInfo[2];
wheelInfo2.m_last_linear_velocity = wheelInfo2.m_linear_velocity;
btVector3 relpos2 = wheelInfo2.m_raycastInfo.m_hardPointWS - getRigidBody()->getCenterOfMassPosition();
btVector3 vel2 = getRigidBody()->getVelocityInLocalPoint(relpos2);
btScalar proj2 = fwd.dot(wheelInfo2.m_raycastInfo.m_contactNormalWS);
btVector3 fwd_w2 = fwd - wheelInfo2.m_raycastInfo.m_contactNormalWS * proj2;
btScalar lin_vel2 = fwd_w2.dot(vel2);
//btScalar w2_rpm = fabs(rads2rpm(lin_vel2/wheelInfo2.m_wheelsRadius));
btScalar delta2 = (lin_vel2/wheelInfo2.m_wheelsRadius)*step;
wheelInfo2.m_linear_velocity = lin_vel2;
wheelInfo2.m_angular_velocity = lin_vel2/wheelInfo2.m_wheelsRadius;
WheelInfo & wheelInfo3 = m_wheelInfo[3];
wheelInfo3.m_last_linear_velocity = wheelInfo3.m_linear_velocity;
btVector3 relpos3 = wheelInfo3.m_raycastInfo.m_hardPointWS - getRigidBody()->getCenterOfMassPosition();
btVector3 vel3 = getRigidBody()->getVelocityInLocalPoint(relpos3);
btScalar proj3 = fwd.dot(wheelInfo3.m_raycastInfo.m_contactNormalWS);
btVector3 fwd_w3 = fwd - wheelInfo3.m_raycastInfo.m_contactNormalWS * proj3;
btScalar lin_vel3 = fwd_w3.dot(vel3);
//btScalar w3_rpm = fabs(rads2rpm(lin_vel3/wheelInfo3.m_wheelsRadius));
btScalar delta3 = (lin_vel3/wheelInfo3.m_wheelsRadius)*step;
wheelInfo3.m_linear_velocity = lin_vel3;
wheelInfo3.m_angular_velocity = lin_vel3/wheelInfo3.m_wheelsRadius;
/////////////////////////////
//
// Update engine
//
//
m_last_engine_rpm = m_engine_rpm;
m_last_throttle = m_throttle;
btScalar to_engine_rpm = btScalar(0.0f);
if (m_differential_type == 0)
{
to_engine_rpm = btMax(wheelInfo2.m_rpm,wheelInfo3.m_rpm);
}
if (m_gear == 1) // idle
{
m_engine_rpm = btScalar(m_rpm_data[m_graphs_size-1]*m_throttle);
}
else
{
m_engine_rpm = to_engine_rpm*m_gears_coefficients[m_gear]*m_final_drive*m_gears_eff;
}
if (m_engine_rpm > btScalar(m_rpm_data[m_graphs_size-1]*m_throttle))
{
m_engine_rpm = m_rpm_data[m_graphs_size-1]*m_throttle;
}
if (m_engine_rpm < btScalar(m_rpm_data[0])) {
m_engine_rpm = btScalar(m_rpm_data[0]);
}
btScalar torque = lookup_value(m_engine_rpm, m_rpm_data,m_torque_data,m_graphs_size)*m_throttle;
#ifdef SHOW_MOTOR_INFO
m_info_engine_torque = torque;
m_info_engine_power = (torque*2.0f*SIMD_PI*m_engine_rpm)/60.0f;
#endif
m_info_engine_rpm = m_engine_rpm + (rand() % 10 + 1);
// drive force to apply to chassis at both rear wheel's
// contact points in local space
btScalar Fdrive;
if (m_gear == 0)
{
Fdrive = (m_gears_coefficients[m_gear]*m_gears_eff*torque)/wheelInfo2.m_wheelsRadius;
Fdrive *= 0.5f; // per wheel
Fdrive *= -1.0f;
}
//if (m_gear == 1)
//{
//Fdrive = btScalar(0.0f);
//}
else
{
Fdrive = (m_gears_coefficients[m_gear]*m_final_drive*m_gears_eff*torque)/wheelInfo2.m_wheelsRadius;
Fdrive *= 0.5f; // per wheel
}
btScalar to_rear_wheel_rpm;
// acceleration (depends on m_inertia_coef) is essential to update engine RPM
btScalar acceleration = (torque*m_gears_coefficients[m_gear]*m_final_drive*m_gears_eff)/m_inertia_coef;
btScalar engine_rpm = m_engine_rpm + rads2rpm(acceleration)*step;
if (m_gear == 0)
{
to_rear_wheel_rpm = engine_rpm/(m_gears_coefficients[m_gear]*m_gears_eff);
to_rear_wheel_rpm *= 0.2f; //?
wheelInfo2.m_engine_force = wheelInfo3.m_engine_force = Fdrive;
wheelInfo2.m_rpm = wheelInfo3.m_rpm = to_rear_wheel_rpm;
if (to_rear_wheel_rpm
>
(m_rpm_data[m_graphs_size-1]*m_throttle)/(m_gears_coefficients[m_gear]*m_final_drive*m_gears_eff))
{
to_rear_wheel_rpm = (m_rpm_data[m_graphs_size-1]*m_throttle)/(m_gears_coefficients[m_gear]*m_final_drive*m_gears_eff);
}
}
else if (m_gear == 1)
{
to_rear_wheel_rpm = 0.0f;
wheelInfo2.m_engine_force = wheelInfo3.m_engine_force = btScalar(0.0f);
wheelInfo2.m_rpm = wheelInfo3.m_rpm = to_rear_wheel_rpm = 0.0f;
}
else
{
to_rear_wheel_rpm = engine_rpm/(m_gears_coefficients[m_gear]*m_final_drive*m_gears_eff);
wheelInfo2.m_engine_force = wheelInfo3.m_engine_force = Fdrive;
wheelInfo2.m_rpm = wheelInfo3.m_rpm = to_rear_wheel_rpm;
if (to_rear_wheel_rpm > (m_rpm_data[m_graphs_size-1]*m_throttle)/(m_gears_coefficients[m_gear]*m_final_drive*m_gears_eff))
{
to_rear_wheel_rpm = (m_rpm_data[m_graphs_size-1]*m_throttle)/(m_gears_coefficients[m_gear]*m_final_drive*m_gears_eff);
}
}
btScalar rear_wheel_delta = rpm2rads(to_rear_wheel_rpm)*step;
//
//
//
/////////////////////////////
/////////////////////////////
//
// Update wheel rotation
//
if (wheelInfo0.m_brake > btScalar(0.0f)) delta0 = btScalar(0.0f);
if (wheelInfo1.m_brake > btScalar(0.0f)) delta1 = btScalar(0.0f);
if (wheelInfo2.m_brake > btScalar(0.0f))
{
delta2 = btScalar(0.0f);
rear_wheel_delta = btScalar(0.0f);
}
if (wheelInfo3.m_brake > btScalar(0.0f))
{
delta3 = btScalar(0.0f);
rear_wheel_delta = btScalar(0.0f);
}
// 0
if (wheelInfo0.m_raycastInfo.m_isInContact)
{
wheelInfo0.m_rotation += delta0;
wheelInfo0.m_rpm = w0_rpm;
}
else
{
wheelInfo0.m_rotation *= 0.99f;
}
// 1
if (wheelInfo1.m_raycastInfo.m_isInContact)
{
wheelInfo1.m_rotation += delta1;
wheelInfo1.m_rpm = w1_rpm;
}
else
{
wheelInfo1.m_rotation *= 0.99f;
}
// 2
if ( wheelInfo2.m_raycastInfo.m_isInContact
&&
( fabs(rear_wheel_delta)
<
fabs(delta2) ) )
{
wheelInfo2.m_rotation += delta2;
}
else
{
if ((m_throttle > btScalar(0.0f)) && (m_gear != 1))
{
if (m_gear == 0)
{
wheelInfo2.m_rotation -= rear_wheel_delta;
}
else
{
wheelInfo2.m_rotation += rear_wheel_delta;
}
}
}
// 3
if ( wheelInfo3.m_raycastInfo.m_isInContact
&&
( fabs(rear_wheel_delta)
<
fabs(delta3) ) )
{
wheelInfo3.m_rotation += delta3;
}
else
{
if ((m_throttle > btScalar(0.0f)) && (m_gear != 1))
{
if (m_gear == 0)
{
wheelInfo3.m_rotation -= rear_wheel_delta;
}
else
{
wheelInfo3.m_rotation += rear_wheel_delta;
}
}
}
//
//
/////////////////////////////
}
void IDDecoyProbability::apply_(vector<PeptideIdentification> & ids, const vector<double> & rev_scores, const vector<double> & fwd_scores, const vector<double> & all_scores)
{
Size number_of_bins(param_.getValue("number_of_bins"));
// normalize distribution to [0, 1]
vector<double> fwd_scores_normalized(number_of_bins, 0.0), rev_scores_normalized(number_of_bins, 0.0), diff_scores(number_of_bins, 0.0), all_scores_normalized(number_of_bins, 0.0);
Transformation_ rev_trafo, fwd_trafo, all_trafo;
normalizeBins_(rev_scores, rev_scores_normalized, rev_trafo);
normalizeBins_(fwd_scores, fwd_scores_normalized, fwd_trafo);
normalizeBins_(all_scores, all_scores_normalized, all_trafo);
// rev scores fitting
vector<DPosition<2> > rev_data;
for (Size i = 0; i < number_of_bins; ++i)
{
DPosition<2> pos;
pos.setX(((double)i) / (double)number_of_bins + 0.0001); // necessary????
pos.setY(rev_scores_normalized[i]);
rev_data.push_back(pos);
#ifdef IDDECOYPROBABILITY_DEBUG
cerr << pos.getX() << " " << pos.getY() << endl;
#endif
}
Math::GammaDistributionFitter gdf;
Math::GammaDistributionFitter::GammaDistributionFitResult result_gamma_1st (1.0, 3.0);
gdf.setInitialParameters(result_gamma_1st);
// TODO heuristic for good start parameters
Math::GammaDistributionFitter::GammaDistributionFitResult result_gamma = gdf.fit(rev_data);
#ifdef IDDECOYPROBABILITY_DEBUG
cerr << gdf.getGnuplotFormula() << endl;
String rev_filename = param_.getValue("rev_filename");
generateDistributionImage_(rev_scores_normalized, gdf.getGnuplotFormula(), rev_filename);
#endif
// generate diffs of distributions
// get the fwd and rev distribution, apply all_trafo and calculate the diff
vector<Size> fwd_bins(number_of_bins, 0), rev_bins(number_of_bins, 0);
double min(all_trafo.min_score), diff(all_trafo.diff_score);
Size max_bin(0);
for (vector<double>::const_iterator it = fwd_scores.begin(); it != fwd_scores.end(); ++it)
{
Size bin = (Size)((*it - min) / diff * (double)(number_of_bins - 1));
++fwd_bins[bin];
if (fwd_bins[bin] > max_bin)
{
max_bin = fwd_bins[bin];
}
}
Size max_reverse_bin(0), max_reverse_bin_value(0);
//min = rev_trafo.min_score;
//diff = rev_trafo.diff_score;
for (vector<double>::const_iterator it = rev_scores.begin(); it != rev_scores.end(); ++it)
{
Size bin = (Size)((*it - min) / diff * (double)number_of_bins);
++rev_bins[bin];
if (rev_bins[bin] > max_bin)
{
max_bin = rev_bins[bin];
}
if (rev_bins[bin] > max_reverse_bin_value)
{
max_reverse_bin = bin;
max_reverse_bin_value = rev_bins[bin];
}
}
#ifdef IDDECOYPROBABILITY_DEBUG
cerr << "Trying to get diff scores" << endl;
#endif
// get diff of fwd and rev
for (Size i = 0; i < number_of_bins; ++i)
{
Size fwd(0), rev(0);
fwd = fwd_bins[i];
rev = rev_bins[i];
if ((double)fwd > (double)(1.3 * rev) && max_reverse_bin < i)
{
diff_scores[i] = (double)(fwd - rev) / (double)max_bin;
}
else
{
diff_scores[i] = 0.0;
}
}
#ifdef IDDECOYPROBABILITY_DEBUG
cerr << "Gauss Fitting values size of diff scores=" << diff_scores.size() << endl;
#endif
// diff scores fitting
vector<DPosition<2> > diff_data;
double gauss_A(0), gauss_x0(0), norm_factor(0);
for (Size i = 0; i < number_of_bins; ++i)
{
DPosition<2> pos;
pos.setX((double)i / (double)number_of_bins);
pos.setY(diff_scores[i]);
if (pos.getY() > gauss_A)
{
gauss_A = pos.getY();
}
gauss_x0 += pos.getX() * pos.getY();
norm_factor += pos.getY();
diff_data.push_back(pos);
}
double gauss_sigma(0);
gauss_x0 /= (double)diff_data.size();
gauss_x0 /= norm_factor;
for (Size i = 0; i <= number_of_bins; ++i)
{
gauss_sigma += fabs(gauss_x0 - (double)i / (double)number_of_bins);
}
gauss_sigma /= (double)diff_data.size();
#ifdef IDDECOYPROBABILITY_DEBUG
cerr << "setting initial parameters: " << endl;
#endif
Math::GaussFitter gf;
Math::GaussFitter::GaussFitResult result_1st(gauss_A, gauss_x0, gauss_sigma);
gf.setInitialParameters(result_1st);
#ifdef IDDECOYPROBABILITY_DEBUG
cerr << "Initial Gauss guess: A=" << gauss_A << ", x0=" << gauss_x0 << ", sigma=" << gauss_sigma << endl;
#endif
//TODO: fail-to-fit correction was done using the GNUPlotFormula. Seemed to be a hack.
//Changed it to try-catch-block but I am not sure if this correction should be made
//at all. Can someone please verify?
Math::GaussFitter::GaussFitResult result_gauss (gauss_A, gauss_x0, gauss_sigma);
try{
result_gauss = gf.fit(diff_data);
}
catch(Exception::UnableToFit& /* e */)
{
result_gauss.A = gauss_A;
result_gauss.x0 = gauss_x0;
result_gauss.sigma = gauss_sigma;
}
// // fit failed?
// if (gf.getGnuplotFormula() == "")
// {
// result_gauss.A = gauss_A;
// result_gauss.x0 = gauss_x0;
// result_gauss.sigma = gauss_sigma;
// }
#ifdef IDDECOYPROBABILITY_DEBUG
cerr << gf.getGnuplotFormula() << endl;
String fwd_filename = param_.getValue("fwd_filename");
if (gf.getGnuplotFormula() == "")
{
String formula("f(x)=" + String(gauss_A) + " * exp(-(x - " + String(gauss_x0) + ") ** 2 / 2 / (" + String(gauss_sigma) + ") ** 2)");
generateDistributionImage_(diff_scores, formula, fwd_filename);
}
else
{
generateDistributionImage_(diff_scores, gf.getGnuplotFormula(), fwd_filename);
}
#endif
#ifdef IDDECOYPROBABILITY_DEBUG
//all_trafo.diff_score + all_trafo.min_score
String gauss_formula("f(x)=" + String(result_gauss.A / all_trafo.max_intensity) + " * exp(-(x - " + String(result_gauss.x0 * all_trafo.diff_score + all_trafo.min_score) + ") ** 2 / 2 / (" + String(result_gauss.sigma * all_trafo.diff_score) + ") ** 2)");
String b_str(result_gamma.b), p_str(result_gamma.p);
String gamma_formula = "g(x)=(" + b_str + " ** " + p_str + ") / gamma(" + p_str + ") * x ** (" + p_str + " - 1) * exp(- " + b_str + " * x)";
generateDistributionImage_(all_scores_normalized, all_trafo, gauss_formula, gamma_formula, (String)param_.getValue("fwd_filename"));
#endif
vector<PeptideIdentification> new_prob_ids;
// calculate the probabilities and write them to the IDs
for (vector<PeptideIdentification>::const_iterator it = ids.begin(); it != ids.end(); ++it)
{
if (it->getHits().size() > 0)
{
vector<PeptideHit> hits;
String score_type = it->getScoreType() + "_score";
for (vector<PeptideHit>::const_iterator pit = it->getHits().begin(); pit != it->getHits().end(); ++pit)
{
PeptideHit hit = *pit;
double score = hit.getScore();
if (!it->isHigherScoreBetter())
{
score = -log10(score);
}
hit.setMetaValue(score_type, hit.getScore());
hit.setScore(getProbability_(result_gamma, rev_trafo, result_gauss, fwd_trafo, score));
hits.push_back(hit);
}
PeptideIdentification id = *it;
id.setHigherScoreBetter(true);
id.setScoreType(id.getScoreType() + "_DecoyProbability");
id.setHits(hits);
new_prob_ids.push_back(id);
}
}
ids = new_prob_ids;
}
// ----------------------------------------------------------------------------
void btKart::updateVehicle( btScalar step )
{
updateAllWheelPositions();
const btTransform& chassisTrans = getChassisWorldTransform();
btVector3 forwardW(chassisTrans.getBasis()[0][m_indexForwardAxis],
chassisTrans.getBasis()[1][m_indexForwardAxis],
chassisTrans.getBasis()[2][m_indexForwardAxis]);
// Simulate suspension
// -------------------
m_num_wheels_on_ground = 0;
m_visual_wheels_touch_ground = true;
for (int i=0;i<m_wheelInfo.size();i++)
{
rayCast( i);
if(m_wheelInfo[i].m_raycastInfo.m_isInContact)
m_num_wheels_on_ground++;
}
// Test if the kart is falling so fast
// that the chassis might hit the track
// ------------------------------------
bool needs_cushioning_test = false;
for(int i=0; i<m_wheelInfo.size(); i++)
{
btWheelInfo &wheel = m_wheelInfo[i];
if(!wheel.m_was_on_ground && wheel.m_raycastInfo.m_isInContact)
{
needs_cushioning_test = true;
break;
}
}
if(needs_cushioning_test)
{
const btVector3 &v = m_chassisBody->getLinearVelocity();
btVector3 down(0, 1, 0);
btVector3 v_down = (v * down) * down;
// Estimate what kind of downward speed can be compensated by the
// suspension. Atm the parameters are set that the suspension is
// actually capped at max suspension force, so the maximum
// speed that can be caught by the suspension without the chassis
// hitting the ground can be based on that. Note that there are
// 4 suspensions, all adding together.
btScalar max_compensate_speed = m_wheelInfo[0].m_maxSuspensionForce
* m_chassisBody->getInvMass()
* step * 4;
// If the downward speed is too fast to be caught by the suspension,
// slow down the falling speed by applying an appropriately impulse:
if(-v_down.getY() > max_compensate_speed)
{
btVector3 impulse = down * (-v_down.getY() - max_compensate_speed)
/ m_chassisBody->getInvMass()*0.5f;
//float v_old = m_chassisBody->getLinearVelocity().getY();
//float x = m_wheelInfo[0].m_raycastInfo.m_isInContact ? m_wheelInfo[0].m_raycastInfo.m_contactPointWS.getY() : -100;
m_chassisBody->applyCentralImpulse(impulse);
//Log::verbose("physics", "Cushioning %f from %f m/s to %f m/s wheel %f kart %f", impulse.getY(),
// v_old, m_chassisBody->getLinearVelocity().getY(), x,
// m_chassisBody->getWorldTransform().getOrigin().getY()
// );
}
}
for(int i=0; i<m_wheelInfo.size(); i++)
m_wheelInfo[i].m_was_on_ground = m_wheelInfo[i].m_raycastInfo.m_isInContact;
// If the kart is flying, try to keep it parallel to the ground.
// -------------------------------------------------------------
if(m_num_wheels_on_ground==0)
{
btVector3 kart_up = getChassisWorldTransform().getBasis().getColumn(1);
btVector3 terrain_up =
m_kart->getMaterial() && m_kart->getMaterial()->hasGravity() ?
m_kart->getNormal() : Vec3(0, 1, 0);
// Length of axis depends on the angle - i.e. the further awat
// the kart is from being upright, the larger the applied impulse
// will be, resulting in fast changes when the kart is on its
// side, but not overcompensating (and therefore shaking) when
// the kart is not much away from being upright.
btVector3 axis = kart_up.cross(terrain_up);
// To avoid the kart going backwards/forwards (or rolling sideways),
// set the pitch/roll to 0 before applying the 'straightening' impulse.
// TODO: make this works if gravity is changed.
btVector3 av = m_chassisBody->getAngularVelocity();
av.setX(0);
av.setZ(0);
m_chassisBody->setAngularVelocity(av);
// Give a nicely balanced feeling for rebalancing the kart
m_chassisBody->applyTorqueImpulse(axis * m_kart->getKartProperties()->getStabilitySmoothFlyingImpulse());
}
// Work around: make sure that either both wheels on one axis
// are on ground, or none of them. This avoids the problem of
// the kart suddenly getting additional angular velocity because
// e.g. only one rear wheel is on the ground and then the kart
// rotates very abruptly.
for(int i=0; i<m_wheelInfo.size(); i+=2)
{
if( m_wheelInfo[i ].m_raycastInfo.m_isInContact !=
m_wheelInfo[i+1].m_raycastInfo.m_isInContact)
{
int wheel_air_index = i;
int wheel_ground_index = i+1;
if (m_wheelInfo[i].m_raycastInfo.m_isInContact)
{
wheel_air_index = i+1;
wheel_ground_index = i;
}
btWheelInfo& wheel_air = m_wheelInfo[wheel_air_index];
btWheelInfo& wheel_ground = m_wheelInfo[wheel_ground_index];
wheel_air.m_raycastInfo = wheel_ground.m_raycastInfo;
}
} // for i=0; i<m_wheelInfo.size(); i+=2
// Apply suspension forcen (i.e. upwards force)
// --------------------------------------------
updateSuspension(step);
for (int i=0;i<m_wheelInfo.size();i++)
{
//apply suspension force
btWheelInfo& wheel = m_wheelInfo[i];
btScalar suspensionForce = wheel.m_wheelsSuspensionForce;
if (suspensionForce > wheel.m_maxSuspensionForce)
{
suspensionForce = wheel.m_maxSuspensionForce;
}
btVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS
* suspensionForce * step;
btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS
- getRigidBody()->getCenterOfMassPosition();
getRigidBody()->applyImpulse(impulse, relpos);
}
// Update friction (i.e. forward force)
// ------------------------------------
updateFriction( step);
for (int i=0;i<m_wheelInfo.size();i++)
{
btWheelInfo& wheel = m_wheelInfo[i];
//btVector3 relpos = wheel.m_raycastInfo.m_hardPointWS
// - getRigidBody()->getCenterOfMassPosition();
//btVector3 vel = getRigidBody()->getVelocityInLocalPoint(relpos);
if (wheel.m_raycastInfo.m_isInContact)
{
const btTransform& chassisWorldTransform =
getChassisWorldTransform();
btVector3 fwd (
chassisWorldTransform.getBasis()[0][m_indexForwardAxis],
chassisWorldTransform.getBasis()[1][m_indexForwardAxis],
chassisWorldTransform.getBasis()[2][m_indexForwardAxis]);
btScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS);
fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj;
}
}
// If configured, add a force to keep karts on the track
// -----------------------------------------------------
float dif = m_kart->getKartProperties()->getStabilityDownwardImpulseFactor();
if(dif!=0 && m_num_wheels_on_ground==4)
{
float f = -fabsf(m_kart->getSpeed()) * dif;
btVector3 downwards_impulse = m_chassisBody->getWorldTransform().getBasis()
* btVector3(0, f, 0);
m_chassisBody->applyCentralImpulse(downwards_impulse);
}
// Apply additional impulse set by supertuxkart
// --------------------------------------------
if(m_time_additional_impulse>0)
{
float dt = step > m_time_additional_impulse
? m_time_additional_impulse
: step;
m_chassisBody->applyCentralImpulse(m_additional_impulse*dt);
m_time_additional_impulse -= dt;
}
// Apply additional rotation set by supertuxkart
// ---------------------------------------------
if(m_time_additional_rotation>0)
{
btTransform &t = m_chassisBody->getWorldTransform();
float dt = step > m_time_additional_rotation
? m_time_additional_rotation
: step;
btQuaternion add_rot(m_additional_rotation.getY()*dt,
m_additional_rotation.getX()*dt,
m_additional_rotation.getZ()*dt);
t.setRotation(t.getRotation()*add_rot);
m_chassisBody->setWorldTransform(t);
// Also apply the rotation to the interpolated world transform.
// This is important (at least if the rotation is only applied
// in one frame) since STK will actually use the interpolated
// transform, which would otherwise only be updated one frame
// later, resulting in a one-frame incorrect rotation of the
// kart, or a strongly 'visual jolt' of the kart
btTransform &iwt=m_chassisBody->getInterpolationWorldTransform();
iwt.setRotation(iwt.getRotation()*add_rot);
m_time_additional_rotation -= dt;
}
} // updateVehicle
void VehicleBody::_direct_state_changed(Object *p_state) {
RigidBody::_direct_state_changed(p_state);
state = Object::cast_to<PhysicsDirectBodyState>(p_state);
float step = state->get_step();
for (int i = 0; i < wheels.size(); i++) {
_update_wheel(i, state);
}
for (int i = 0; i < wheels.size(); i++) {
_ray_cast(i, state);
wheels[i]->set_transform(state->get_transform().inverse() * wheels[i]->m_worldTransform);
}
_update_suspension(state);
for (int i = 0; i < wheels.size(); i++) {
//apply suspension force
VehicleWheel &wheel = *wheels[i];
real_t suspensionForce = wheel.m_wheelsSuspensionForce;
if (suspensionForce > wheel.m_maxSuspensionForce) {
suspensionForce = wheel.m_maxSuspensionForce;
}
Vector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step;
Vector3 relpos = wheel.m_raycastInfo.m_contactPointWS - state->get_transform().origin;
state->apply_impulse(relpos, impulse);
//getRigidBody()->applyImpulse(impulse, relpos);
}
_update_friction(state);
for (int i = 0; i < wheels.size(); i++) {
VehicleWheel &wheel = *wheels[i];
Vector3 relpos = wheel.m_raycastInfo.m_hardPointWS - state->get_transform().origin;
Vector3 vel = state->get_linear_velocity() + (state->get_angular_velocity()).cross(relpos); // * mPos);
if (wheel.m_raycastInfo.m_isInContact) {
const Transform &chassisWorldTransform = state->get_transform();
Vector3 fwd(
chassisWorldTransform.basis[0][Vector3::AXIS_Z],
chassisWorldTransform.basis[1][Vector3::AXIS_Z],
chassisWorldTransform.basis[2][Vector3::AXIS_Z]);
real_t proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS);
fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj;
real_t proj2 = fwd.dot(vel);
wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelRadius);
wheel.m_rotation += wheel.m_deltaRotation;
} else {
wheel.m_rotation += wheel.m_deltaRotation;
}
wheel.m_deltaRotation *= real_t(0.99); //damping of rotation when not in contact
}
state = NULL;
}
int SmoTutor::takeStep(int i1, int i2, double e2)
{
if (i1 == i2)
{
return 0;
}
/* compute upper and lower constraints, L and H, on multiplier a2 */
double alpha1 = alpha[i1];
double alpha2 = alpha[i2];
double y1 = y[i1];
double y2 = y[i2];
double L;
double H;
if (y1 != y2)
{
L = max(0, alpha2 - alpha1);
H = min(C[i2], alpha2 - alpha1 + C[i1]);
}
else
{
L = max(0, alpha1 + alpha2 - C[i1]);
H = min(C[i2], alpha1 + alpha2);
}
if (L == H)
{
return 0;
}
/* recompute Lagrange multiplier for pattern i2 */
double e1;
if (alpha1 > 0.0 && alpha1 < C[i1])
{
e1 = error[i1];
}
else
{
e1 = fwd(i1) - y1;
}
double k11 = cache->fetch(i1, i1);
double k12 = cache->fetch(i1, i2);
double k22 = cache->fetch(i2, i2);
double eta = 2.0*k12-k11-k22;
double s = y1*y2;
double a2 = 0.0;
if (eta < 0.0)
{
a2 = alpha2 - y2*(e1 - e2)/eta;
/* constrain a2 to lie between L and H */
if (a2 < L)
{
a2 = L;
}
else if (a2 > H)
{
a2 = H;
}
}
else
{
return 0;
}
if (fabs(a2-alpha2) < epsilon*(a2+alpha2+epsilon))
{
return 0;
}
/* recompute Lagrange multiplier for pattern i1 */
double a1 = alpha1+s*(alpha2-a2);
/* update vector of Lagrange multipliers */
alpha[i1] = a1;
alpha[i2] = a2;
/* update threshold to reflect change in Lagrange multipliers */
double w1 = y1*(a1 - alpha1);
double w2 = y2*(a2 - alpha2);
double b1 = e1 + w1*k11 + w2*k12;
double b2 = e2 + w1*k12 + w2*k22;
double bold = bias;
bias += 0.5*(b1 + b2);
/* update error cache->*/
if (fabs(b1-b2) < epsilon)
{
error[i1] = 0.0;
error[i2] = 0.0;
}
else
{
if (a1 > 0.0 && a1 < C[i1])
{
error[i1] = fwd(i1) - y1;
}
if (a2 > 0.0 && a2 < C[i2])
{
error[i2] = fwd(i2) - y2;
}
}
if (error[i1] > error[i2])
{
minimum = i2;
maximum = i1;
}
else
{
minimum = i1;
maximum = i2;
}
for (int i = 0; i < ntp; i++)
{
if (alpha[i] > 0.0 && alpha[i] < C[i] && i != i1 && i != i2)
{
error[i] += w1*cache->fetch(i1, i)
+ w2*cache->fetch(i2, i)
+ bold - bias;
if (error[i] > error[maximum])
{
maximum = i;
}
if (error[i] < error[minimum])
{
minimum = i;
}
}
}
/* report progress made */
return 1;
}
void btRaycastVehicle::updateVehicle( btScalar step )
{
{
for (int i=0;i<getNumWheels();i++)
{
updateWheelTransform(i,false);
}
}
m_currentVehicleSpeedKmHour = btScalar(3.6) * getRigidBody()->getLinearVelocity().length();
const btTransform& chassisTrans = getChassisWorldTransform();
btVector3 forwardW (
chassisTrans.getBasis()[0][m_indexForwardAxis],
chassisTrans.getBasis()[1][m_indexForwardAxis],
chassisTrans.getBasis()[2][m_indexForwardAxis]);
if (forwardW.dot(getRigidBody()->getLinearVelocity()) < btScalar(0.))
{
m_currentVehicleSpeedKmHour *= btScalar(-1.);
}
//
// simulate suspension
//
int i=0;
for (i=0;i<m_wheelInfo.size();i++)
{
btScalar depth;
depth = rayCast( m_wheelInfo[i]);
}
updateSuspension(step);
for (i=0;i<m_wheelInfo.size();i++)
{
//apply suspension force
btWheelInfo& wheel = m_wheelInfo[i];
btScalar suspensionForce = wheel.m_wheelsSuspensionForce;
if (suspensionForce > wheel.m_maxSuspensionForce)
{
suspensionForce = wheel.m_maxSuspensionForce;
}
btVector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step;
btVector3 relpos = wheel.m_raycastInfo.m_contactPointWS - getRigidBody()->getCenterOfMassPosition();
getRigidBody()->applyImpulse(impulse, relpos);
}
updateFriction( step);
for (i=0;i<m_wheelInfo.size();i++)
{
btWheelInfo& wheel = m_wheelInfo[i];
btVector3 relpos = wheel.m_raycastInfo.m_hardPointWS - getRigidBody()->getCenterOfMassPosition();
btVector3 vel = getRigidBody()->getVelocityInLocalPoint( relpos );
if (wheel.m_raycastInfo.m_isInContact)
{
const btTransform& chassisWorldTransform = getChassisWorldTransform();
btVector3 fwd (
chassisWorldTransform.getBasis()[0][m_indexForwardAxis],
chassisWorldTransform.getBasis()[1][m_indexForwardAxis],
chassisWorldTransform.getBasis()[2][m_indexForwardAxis]);
btScalar proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS);
fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj;
btScalar proj2 = fwd.dot(vel);
wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelsRadius);
wheel.m_rotation += wheel.m_deltaRotation;
} else
{
wheel.m_rotation += wheel.m_deltaRotation;
}
wheel.m_deltaRotation *= btScalar(0.99);//damping of rotation when not in contact
}
}
void Edge::solveEdgeEqs()
{
fwd();
bwd();
}
int main( int argc, char *argv[] ) {
config conf;
setup_config(&conf, argc, argv);
init_progress(conf.progress_width, conf.nsteps, conf.progress_disabled);
TYPE dx = 20.f;
TYPE dt = 0.002f;
// compute the pitch for perfect coalescing
size_t dimx = conf.nx + 2*conf.radius;
size_t dimy = conf.ny + 2*conf.radius;
size_t nbytes = dimx * dimy * sizeof(TYPE);
if (conf.verbose) {
printf("x = %zu, y = %zu\n", dimx, dimy);
printf("nsteps = %d\n", conf.nsteps);
printf("radius = %d\n", conf.radius);
}
TYPE c_coeff[NUM_COEFF];
TYPE *curr = (TYPE *)malloc(nbytes);
TYPE *next = (TYPE *)malloc(nbytes);
TYPE *vsq = (TYPE *)malloc(nbytes);
if (curr == NULL || next == NULL || vsq == NULL) {
fprintf(stderr, "Allocations failed\n");
return 1;
}
config_sources(&conf.srcs, &conf.nsrcs, conf.nx, conf.ny, conf.nsteps);
TYPE **srcs = sample_sources(conf.srcs, conf.nsrcs, conf.nsteps, dt);
init_data(curr, next, vsq, c_coeff, dimx, dimy, dx, dt);
double start = seconds();
for (int step = 0; step < conf.nsteps; step++) {
for (int src = 0; src < conf.nsrcs; src++) {
if (conf.srcs[src].t > step) continue;
int src_offset = POINT_OFFSET(conf.srcs[src].x, conf.srcs[src].y,
dimx, conf.radius);
curr[src_offset] = srcs[src][step];
}
fwd(next, curr, vsq, c_coeff, conf.nx, conf.ny, dimx, dimy,
conf.radius);
TYPE *tmp = next;
next = curr;
curr = tmp;
update_progress(step + 1);
}
double elapsed_s = seconds() - start;
float point_rate = (float)conf.nx * conf.ny / (elapsed_s / conf.nsteps);
printf("iso_r4_2x: %8.10f s total, %8.10f s/step, %8.2f Mcells/s/step\n",
elapsed_s, elapsed_s / conf.nsteps, point_rate / 1000000.f);
if (conf.save_text) {
save_text(curr, dimx, dimy, conf.ny, conf.nx, "snap.text", conf.radius);
}
free(curr);
free(next);
free(vsq);
for (int i = 0; i < conf.nsrcs; i++) {
free(srcs[i]);
}
free(srcs);
return 0;
}
int main()
{
//=====================================================================
// Initialize variables
//=====================================================================
int running,n=0,arg,time=0;
double dist=0,angle=0;
count = 1;
reg_k = 0.35;
//=====================================================================
// Establish connection to robot sensors and actuators.
//=====================================================================
if (rhdConnect('w',"localhost",ROBOTPORT)!='w'){
printf("Can't connect to rhd \n");
exit(EXIT_FAILURE);
}
printf("connected to robot \n");
if ((inputtable=getSymbolTable('r'))== NULL){
printf("Can't connect to rhd \n");
exit(EXIT_FAILURE);
}
if ((outputtable=getSymbolTable('w'))== NULL){
printf("Can't connect to rhd \n");
exit(EXIT_FAILURE);
}
// connect to robot I/O variables
lenc=getinputref("encl",inputtable);
renc=getinputref("encr",inputtable);
linesensor=getinputref("linesensor",inputtable);
irsensor=getinputref("irsensor",inputtable);
speedl=getoutputref("speedl",outputtable);
speedr=getoutputref("speedr",outputtable);
resetmotorr=getoutputref("resetmotorr",outputtable);
resetmotorl=getoutputref("resetmotorl",outputtable);
//=====================================================================
// Camera server code initialization
//=====================================================================
/* Create endpoint */
lmssrv.port=24919;
strcpy(lmssrv.host,"127.0.0.1");
strcpy(lmssrv.name,"laserserver");
lmssrv.status=1;
camsrv.port=24920;
strcpy(camsrv.host,"127.0.0.1");
camsrv.config=1;
strcpy(camsrv.name,"cameraserver");
camsrv.status=1;
if (camsrv.config) {
int errno = 0;
camsrv.sockfd = socket(AF_INET, SOCK_STREAM, 0);
if ( camsrv.sockfd < 0 )
{
perror(strerror(errno));
fprintf(stderr," Can not make socket\n");
exit(errno);
}
serverconnect(&camsrv);
xmldata=xml_in_init(4096,32);
printf(" camera server xml initialized \n");
}
//=====================================================================
// LMS server code initialization
//=====================================================================
/* Create endpoint */
lmssrv.config=1;
if (lmssrv.config) {
char buf[256];
int errno = 0,len;
lmssrv.sockfd = socket(AF_INET, SOCK_STREAM, 0);
if ( lmssrv.sockfd < 0 )
{
perror(strerror(errno));
fprintf(stderr," Can not make socket\n");
exit(errno);
}
serverconnect(&lmssrv);
if (lmssrv.connected){
xmllaser=xml_in_init(4096,32);
printf(" laserserver xml initialized \n");
len=sprintf(buf,"scanpush cmd='zoneobst'\n");
send(lmssrv.sockfd,buf,len,0);
}
}
/* Read sensors and zero our position.
*/
rhdSync();
odo.w=WHEEL_SEPARATION;
odo.cr=DELTA_M;
odo.cl=odo.cr;
odo.left_enc=lenc->data[0];
odo.right_enc=renc->data[0];
reset_odo(&odo);
running=1;
mission.state=ms_init;
mission.oldstate=-1;
reset_sen(&sen);
//=====================================================================
// Main program loop
//=====================================================================
while (running){
if (lmssrv.config && lmssrv.status && lmssrv.connected){
while ( (xml_in_fd(xmllaser,lmssrv.sockfd) >0))
xml_proca(xmllaser);
}
if (camsrv.config && camsrv.status && camsrv.connected){
while ( (xml_in_fd(xmldata,camsrv.sockfd) >0))
xml_proc(xmldata);
}
rhdSync();
update_odo(&odo);
update_sen(&sen);
update_motcon(&mot);
//=====================================================================
// Mission reading function !!!!!!MAKE!!!!!
//=====================================================================
if(fwd(3,0.5,time,&mot) == 1)
running = 0;
//printf("%f %f %f %f %f %d %d %d\n",sen.irarr[0],sen.irarr[1],sen.irarr[2],sen.irarr[3],sen.irarr[4]);
speedl->data[0]=100*mot.motorspeed_l;
speedl->updated=1;
speedr->data[0]=100*mot.motorspeed_r;
speedr->updated=1;
if (time % 100 ==0)
time++;
/* stop if keyboard is activated
*
*/
ioctl(0, FIONREAD, &arg);
if (arg!=0) running=0;
}/* end of main control loop */
speedl->data[0]=0;
speedl->updated=1;
speedr->data[0]=0;
speedr->updated=1;
rhdSync();
rhdDisconnect();
exit(0);
}
