int main() { // Set the image rotation (in degrees) float theta = 3.14159/6; float cos_theta = cosf(theta); float sin_theta = sinf(theta); printf("theta = %f (cos theta = %f, sin theta = %f)\n", theta, cos_theta, sin_theta); // Rows and columns in the input image int imageHeight; int imageWidth; const char* inputFile = "input.bmp"; const char* outputFile = "output.bmp"; // Homegrown function to read a BMP from file float* inputImage = readImage(inputFile, &imageWidth, &imageHeight); // Size of the input and output images on the host int dataSize = imageHeight*imageWidth*sizeof(float); // Output image on the host float* outputImage = NULL; outputImage = (float*)malloc(dataSize); // Set up the OpenCL environment cl_int status; // Discovery platform cl_platform_id platforms[2]; cl_platform_id platform; status = clGetPlatformIDs(2, platforms, NULL); chk(status, "clGetPlatformIDs"); platform = platforms[PLATFORM_TO_USE]; // Discover device cl_device_id device; clGetDeviceIDs(platform, CL_DEVICE_TYPE_CPU, 1, &device, NULL); chk(status, "clGetDeviceIDs"); // Create context cl_context_properties props[3] = {CL_CONTEXT_PLATFORM, (cl_context_properties)(platform), 0}; cl_context context; context = clCreateContext(props, 1, &device, NULL, NULL, &status); chk(status, "clCreateContext"); // Create command queue cl_command_queue queue; queue = clCreateCommandQueue(context, device, 0, &status); chk(status, "clCreateCommandQueue"); // Create the input and output buffers cl_mem d_input; d_input = clCreateBuffer(context, CL_MEM_READ_ONLY, dataSize, NULL, &status); chk(status, "clCreateBuffer"); cl_mem d_output; d_output = clCreateBuffer(context, CL_MEM_WRITE_ONLY, dataSize, NULL, &status); chk(status, "clCreateBuffer"); // Copy the input image to the device status = clEnqueueWriteBuffer(queue, d_input, CL_TRUE, 0, dataSize, inputImage, 0, NULL, NULL); chk(status, "clEnqueueWriteBuffer"); const char* source = readSource("rotation.cl"); // Create a program object with source and build it cl_program program; program = clCreateProgramWithSource(context, 1, &source, NULL, NULL); chk(status, "clCreateProgramWithSource"); status = clBuildProgram(program, 1, &device, NULL, NULL, NULL); chk(status, "clBuildProgram"); // Create the kernel object cl_kernel kernel; kernel = clCreateKernel(program, "img_rotate", &status); chk(status, "clCreateKernel"); // Set the kernel arguments status = clSetKernelArg(kernel, 0, sizeof(cl_mem), &d_output); status |= clSetKernelArg(kernel, 1, sizeof(cl_mem), &d_input); status |= clSetKernelArg(kernel, 2, sizeof(int), &imageWidth); status |= clSetKernelArg(kernel, 3, sizeof(int), &imageHeight); status |= clSetKernelArg(kernel, 4, sizeof(float), &sin_theta); status |= clSetKernelArg(kernel, 5, sizeof(float), &cos_theta); chk(status, "clSetKernelArg"); // Set the work item dimensions size_t globalSize[2] = {imageWidth, imageHeight}; status = clEnqueueNDRangeKernel(queue, kernel, 2, NULL, globalSize, NULL, 0, NULL, NULL); chk(status, "clEnqueueNDRange"); // Read the image back to the host status = clEnqueueReadBuffer(queue, d_output, CL_TRUE, 0, dataSize, outputImage, 0, NULL, NULL); chk(status, "clEnqueueReadBuffer"); // Write the output image to file storeImage(outputImage, outputFile, imageHeight, imageWidth, inputFile); return 0; }
void ofxChartSeriesPieSlice::setupSlice(int pixelOffset, int radius, float _percentage,float height, ofColor color) { percent = _percentage; int numVertices = _percentage * 360 ; //setup topface mFace.addVertex(ofVec3f(0,0,0)); mFace.addNormal(ofVec3f(0,0,1)); mFace.addColor(color); for (int i = 0; i < numVertices; i++) { float x = radius * cosf(ofDegToRad(i)); float y = radius * sinf(ofDegToRad(i)); mFace.addVertex(ofVec3f(x,y,0)); mFace.addNormal(ofVec3f(0,0,1)); mFace.addColor(color); } mFace.setMode(OF_PRIMITIVE_TRIANGLE_FAN); mBottom.addVertex(ofVec3f(0,0,height)); mBottom.addNormal(ofVec3f(0,0,1)); mBottom.addColor(color); for (int i = 0; i < numVertices; i++) { float x = radius * cosf(ofDegToRad(i)); float y = radius * sinf(ofDegToRad(i)); mBottom.addVertex(ofVec3f(x,y,height)); mBottom.addNormal(ofVec3f(0,0,1)); mBottom.addColor(color); } mBottom.setMode(OF_PRIMITIVE_TRIANGLE_FAN); vector<ofVec3f>edgeVertices = vector<ofVec3f>(numVertices*2) ; vector<ofVec3f>edgeNormals = vector<ofVec3f>(numVertices*2) ; //calculate edge color ofColor edgeColor = color/2; mEdge.addVertex(ofVec3f(0,0,0)); mEdge.addVertex(ofVec3f(0,0,height)); mEdge.addNormal(ofVec3f(0,0,1)); mEdge.addNormal(ofVec3f(0,0,1)); mEdge.addColor(edgeColor); mEdge.addColor(edgeColor); //setup edge for (int i = 0; i < numVertices; i++) { float x = radius * cosf(ofDegToRad(i)); float y = radius * sinf(ofDegToRad(i)); edgeVertices[2 * i] = ofVec3f(x, y, 0); edgeVertices[2 * i + 1] = ofVec3f(x, y, height); } for (int i = 0; i < (numVertices * 2)-3; i++) { ofVec3f v1 = edgeVertices[i]; ofVec3f v2 = edgeVertices[i + 1]; ofVec3f v3 = edgeVertices[i + 2]; mEdge.addColor(edgeColor); if (i % 2 == 0) { ofVec3f u1 =v2- v1; //vector product ofVec3f u2 = v3-v1; ofVec3f normal = normalize(u1.cross(u2)); //normalized cross product edgeNormals[i] = normal; edgeNormals[i + 1] = normal; } } //add results to mesh mEdge.addVertices(edgeVertices); mEdge.addNormals(edgeNormals); mEdge.setMode(OF_PRIMITIVE_TRIANGLE_STRIP); //? }
int main(int argc, const char * argv[]) { if(!glfwInit()){ return -1; } glfwWindowHint(GLFW_CONTEXT_VERSION_MAJOR, 3); glfwWindowHint(GLFW_CONTEXT_VERSION_MINOR, 3); glfwWindowHint(GLFW_RESIZABLE, GL_FALSE); glfwWindowHint(GLFW_OPENGL_PROFILE, GLFW_OPENGL_CORE_PROFILE); glfwWindowHint(GLFW_OPENGL_FORWARD_COMPAT, GL_TRUE); auto window = glfwCreateWindow(WIDTH, HEIGHT, "Transformation", nullptr, nullptr); if (nullptr == window) { std::cout << "Failed to create GLFW windows" << std::endl; glfwTerminate(); return -1; } glfwMakeContextCurrent(window); int actualWidth; int actualHeight; glfwGetFramebufferSize(window, &actualWidth, &actualHeight); glViewport(0, 0, actualWidth, actualHeight); glfwSetKeyCallback(window, key_callback); GLint nrAttributes; glGetIntegerv(GL_MAX_VERTEX_ATTRIBS, &nrAttributes); std::cout << "Maximum number of vertex attributes supported: " << nrAttributes << std::endl; GLuint shaderProgram = createShaderProgramWithFilenames("vertex.vsh", "fragment.fsh"); GLfloat vertices[] = { // Positions // Colors // Texture Coords 0.5f, 0.5f, 0.0f, 1.0f, 0.0f, 0.0f, 1.f, 1.0f, // Top Right 0.5f, -0.5f, 0.0f, 0.0f, 1.0f, 0.0f, 1.0f, 0.f, // Bottom Right -0.5f, -0.5f, 0.0f, 0.0f, 0.0f, 1.0f, 0.f, 0.f, // Bottom Left -0.5f, 0.5f, 0.0f, 1.0f, 1.0f, 0.0f, 0.f, 1.f // Top Left }; GLuint indices[] = { // Note that we start from 0! 0, 1, 3, // First Triangle 1, 2, 3 // Second Triangle }; GLuint VAO; glGenVertexArrays(1, &VAO); glBindVertexArray(VAO); GLuint VBO; glGenBuffers(1, &VBO); glBindBuffer(GL_ARRAY_BUFFER, VBO); glBufferData(GL_ARRAY_BUFFER, sizeof(vertices), vertices, GL_STATIC_DRAW); GLuint EBO; glGenBuffers(1, &EBO); glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, EBO); glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(indices), indices, GL_STATIC_DRAW); glVertexAttribPointer(0, 3, GL_FLOAT, GL_FALSE, 8 * sizeof(GLfloat), (GLvoid*)0); glEnableVertexAttribArray(0); glVertexAttribPointer(1, 3, GL_FLOAT, GL_FALSE, 8 * sizeof(GLfloat), (GLvoid*)(3 * sizeof(GLfloat))); glEnableVertexAttribArray(1); glVertexAttribPointer(2, 2, GL_FLOAT, GL_FALSE, 8 * sizeof(GLfloat), (GLvoid*)(6 * sizeof(GLfloat))); glEnableVertexAttribArray(2); glBindVertexArray(0); int width, height; unsigned char* image = SOIL_load_image("container.jpg", &width, &height, 0, SOIL_LOAD_RGB); GLuint texture; glGenTextures(1, &texture); glBindTexture(GL_TEXTURE_2D, texture); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_RGB, GL_UNSIGNED_BYTE, image); glGenerateMipmap(GL_TEXTURE_2D); SOIL_free_image_data(image); glBindTexture(GL_TEXTURE_2D, 0); unsigned char* image2 = SOIL_load_image("awesomeface.png", &width, &height, 0, SOIL_LOAD_RGB); GLuint texture2; glGenTextures(1, &texture2); glBindTexture(GL_TEXTURE_2D, texture2); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, width, height, 0, GL_RGB, GL_UNSIGNED_BYTE, image2); glGenerateMipmap(GL_TEXTURE_2D); SOIL_free_image_data(image2); glBindTexture(GL_TEXTURE_2D, 0); // glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); while (!glfwWindowShouldClose(window)) { glfwPollEvents(); //do rendering glClearColor(0.2, 0.3, 0.3, 1.0); glClear(GL_COLOR_BUFFER_BIT); glUseProgram(shaderProgram); glActiveTexture(GL_TEXTURE0); glBindTexture(GL_TEXTURE_2D, texture); glUniform1i(glGetUniformLocation(shaderProgram, "ourTexture1"), 0); glActiveTexture(GL_TEXTURE1); glBindTexture(GL_TEXTURE_2D, texture2); glUniform1i(glGetUniformLocation(shaderProgram, "ourTexture2"), 1); glUniform1f(glGetUniformLocation(shaderProgram, "mixValue"), mixValue); glm::mat4 trans; trans = glm::translate(trans, glm::vec3(0.5f, -0.5f, 0.0f)); trans = glm::rotate(trans, glm::radians((GLfloat)glfwGetTime() * 50.0f), glm::vec3(0.0f, 0.0f, 1.0f)); GLuint transformLoc = glGetUniformLocation(shaderProgram, "transforms"); glUniformMatrix4fv(transformLoc, 1, GL_FALSE, glm::value_ptr(trans)); //draw the first triangles glBindVertexArray(VAO); glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_INT, 0); trans = glm::mat4(); //reset the previous matrix trans = glm::translate(trans, glm::vec3(-0.5f, 0.5f, 0.0f)); float scaleFactor = sinf((GLfloat)glfwGetTime()) ; trans = glm::scale(trans,glm::vec3(scaleFactor, scaleFactor, 1.0f )); glUniformMatrix4fv(transformLoc, 1, GL_FALSE, glm::value_ptr(trans)); glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_INT, 0); glBindVertexArray(0); glfwSwapBuffers(window); } glDeleteVertexArrays(1, &VAO); glDeleteBuffers(1, &VBO); glDeleteBuffers(1, &EBO); glDeleteProgram(shaderProgram); glfwTerminate(); return 0; }
int ScriptMonitor::sin(int angle) { return (int)(1000*sinf((float)angle)*M_PI/180.0); }
void voltage_switch_inverter_VSI(int S_A, int S_B, int S_C) { /* float duty_a=1.0f; float duty_b=1.0f; float duty_c=1.0f; float attenuation =1.0f; */ close_loop=true; cur_angle+=2.0f*PI*TICK_PERIOD*ref_freq; //converting big angles into something between 0 and 2pi if (cur_angle >= (2.0f*PI)) { cur_angle=cur_angle-(2.0f*PI); } //close_loop=false; if (!close_loop) { duty_a=sinf(cur_angle); duty_b=sinf(cur_angle+2.0f*PI/3.0f); duty_c=sinf(cur_angle+4.0f*PI/3.0f); } else { duty_a=1.0f; duty_b=1.0f; duty_c=1.0f; attenuation =1.0f;//0.5f;//1.0f; } if (motor_off) { duty_a=0.0f; duty_b=0.0f; duty_c=0.0f; attenuation=1.0f; } /* //#define CURRENT_LIMIT 14.0f if ( i_sA >CURRENT_LIMIT || i_sA <-CURRENT_LIMIT || i_sB >CURRENT_LIMIT || i_sB <-CURRENT_LIMIT || (-i_sA-i_sB)>CURRENT_LIMIT || (-i_sA-i_sB)<-CURRENT_LIMIT) { duty_a=0.0f; duty_b=0.0f; duty_c=0.0f; attenuation=1.0f; motor_stop=true; //printf("\n\nMotor off, overcurrent...\n\n"); } */ /* //PWM mode TIM_OCM_FROZEN, TIM_OCM_ACTIVE, TIM_OCM_INACTIVE, TIM_OCM_TOGGLE, TIM_OCM_FORCE_LOW, TIM_OCM_FORCE_HIGH, TIM_OCM_PWM1, TIM_OCM_PWM2, */ //dtc switching selection if (close_loop) { //----------------SA: S1 and S4--------------------------------- if (S_A==1) { timer_set_oc_mode(TIM1, TIM_OC1, TIM_OCM_PWM1); //timer_set_oc_mode (TIM1, TIM_OC1, TIM_OCM_FORCE_HIGH); timer_enable_oc_output (TIM1, TIM_OC1 ); //S1 on timer_disable_oc_output (TIM1, TIM_OC1N); //S4 off } else if (S_A==0) { timer_set_oc_mode(TIM1, TIM_OC1, TIM_OCM_PWM1); //timer_set_oc_mode (TIM1, TIM_OC1, TIM_OCM_FORCE_HIGH); timer_disable_oc_output (TIM1, TIM_OC1); //S1 off timer_enable_oc_output (TIM1, TIM_OC1N); //S4 on } else { duty_a=0.0f; timer_set_oc_mode(TIM1, TIM_OC1, TIM_OCM_PWM1); //timer_set_oc_mode (TIM1, TIM_OC1, TIM_OCM_FORCE_HIGH); timer_disable_oc_output (TIM1, TIM_OC1); //S1 off timer_disable_oc_output (TIM1, TIM_OC1N); //S4 on } //-------------SB: S3 and S6------------------------------------ if (S_B==1) { timer_set_oc_mode(TIM1, TIM_OC2, TIM_OCM_PWM1); //timer_set_oc_mode(TIM1, TIM_OC2, TIM_OCM_FORCE_HIGH); timer_enable_oc_output(TIM1, TIM_OC2 ); //S3 on timer_disable_oc_output (TIM1, TIM_OC2N); //S6 off } else if (S_B==0) { timer_set_oc_mode(TIM1, TIM_OC2, TIM_OCM_PWM1); //timer_set_oc_mode(TIM1, TIM_OC2, TIM_OCM_FORCE_HIGH); timer_disable_oc_output(TIM1, TIM_OC2 ); //S3 off timer_enable_oc_output (TIM1, TIM_OC2N); //S6 on } else { duty_b=0.0f; timer_set_oc_mode(TIM1, TIM_OC2, TIM_OCM_PWM1); //timer_set_oc_mode(TIM1, TIM_OC2, TIM_OCM_FORCE_HIGH); timer_disable_oc_output(TIM1, TIM_OC2 ); //S3 off timer_disable_oc_output (TIM1, TIM_OC2N); //S6 on } //-----------SC: S5 and S2-------------------------------------- if (S_C==1) { timer_set_oc_mode(TIM1, TIM_OC3, TIM_OCM_PWM1); //timer_set_oc_mode(TIM1, TIM_OC3, TIM_OCM_FORCE_HIGH); timer_enable_oc_output(TIM1, TIM_OC3 ); //S5 on timer_disable_oc_output (TIM1, TIM_OC3N); //S2 off } else if (S_C==0) { timer_set_oc_mode(TIM1, TIM_OC3, TIM_OCM_PWM1); //timer_set_oc_mode(TIM1, TIM_OC3, TIM_OCM_FORCE_HIGH); timer_disable_oc_output(TIM1, TIM_OC3 ); //S5 off timer_enable_oc_output (TIM1, TIM_OC3N); //S2 on } else { duty_c=0.0f; timer_set_oc_mode(TIM1, TIM_OC3, TIM_OCM_PWM1); //timer_set_oc_mode(TIM1, TIM_OC3, TIM_OCM_FORCE_HIGH); timer_disable_oc_output(TIM1, TIM_OC3 ); //S5 off timer_disable_oc_output (TIM1, TIM_OC3N); //S2 on } } //open loop switching selection /* else { if (duty_a < 0.0f) { timer_set_oc_mode(TIM1, TIM_OC1, TIM_OCM_PWM1); timer_disable_oc_output(TIM1,TIM_OC1); timer_enable_oc_output (TIM1, TIM_OC1N); duty_a=-duty_a; } else { timer_set_oc_mode(TIM1, TIM_OC1, TIM_OCM_PWM1); timer_enable_oc_output(TIM1, TIM_OC1 ); timer_disable_oc_output (TIM1, TIM_OC1N); } if (duty_b < 0.0f) { timer_set_oc_mode(TIM1, TIM_OC2, TIM_OCM_PWM1); timer_disable_oc_output(TIM1, TIM_OC2 ); timer_enable_oc_output (TIM1, TIM_OC2N); duty_b=-duty_b; } else { timer_set_oc_mode(TIM1, TIM_OC2, TIM_OCM_PWM1); timer_enable_oc_output(TIM1, TIM_OC2 ); timer_disable_oc_output (TIM1, TIM_OC2N); } if (duty_c < 0.0f) { timer_set_oc_mode(TIM1, TIM_OC3, TIM_OCM_PWM1); timer_disable_oc_output(TIM1, TIM_OC3 ); timer_enable_oc_output (TIM1, TIM_OC3N); duty_c=-duty_c; } else { timer_set_oc_mode(TIM1, TIM_OC3, TIM_OCM_PWM1); timer_enable_oc_output(TIM1, TIM_OC3 ); timer_disable_oc_output (TIM1, TIM_OC3N); } } */ /* Set the capture compare value for OC1. */ timer_set_oc_value(TIM1, TIM_OC1, duty_a*attenuation*PWM_PERIOD_ARR); /* Set the capture compare value for OC1. */ timer_set_oc_value(TIM1, TIM_OC2, duty_b*attenuation*PWM_PERIOD_ARR); /* Set the capture compare value for OC1. */ timer_set_oc_value(TIM1, TIM_OC3, duty_c*attenuation*PWM_PERIOD_ARR); //tim_force_update_event(TIM1); }
TriMeshf sphere(const float radius, const ml::vec3f& pos, const size_t stacks /*= 10*/, const size_t slices /*= 10*/, const ml::vec4f& color /*= ml::vec4f(1,1,1,1) */) { MeshDataf meshdata; auto& V = meshdata.m_Vertices; auto& I = meshdata.m_FaceIndicesVertices; auto& N = meshdata.m_Normals; auto& C = meshdata.m_Colors; const float thetaDivisor = 1.0f / stacks * ml::math::PIf; const float phiDivisor = 1.0f / slices * 2.0f * ml::math::PIf; for (size_t t = 0; t < stacks; t++) { // stacks increment elevation (theta) float theta1 = t * thetaDivisor; float theta2 = (t + 1) * thetaDivisor; for (size_t p = 0; p < slices; p++) { // slices increment azimuth (phi) float phi1 = p * phiDivisor; float phi2 = (p + 1) * phiDivisor; const auto sph2xyz = [&](float r, float theta, float phi) { const float sinTheta = sinf(theta), sinPhi = sinf(phi), cosTheta = cosf(theta), cosPhi = cosf(phi); return ml::vec3f(r * sinTheta * cosPhi, r * sinTheta * sinPhi, r * cosTheta); }; // phi2 phi1 // | | // 2------1 -- theta1 // |\ _ | // | \ | // 3------4 -- theta2 // // Points const ml::vec3f c1 = pos + sph2xyz(radius, theta1, phi1), c2 = pos + sph2xyz(radius, theta1, phi2), c3 = pos + sph2xyz(radius, theta2, phi2), c4 = pos + sph2xyz(radius, theta2, phi1); V.push_back(c1); V.push_back(c2); V.push_back(c3); V.push_back(c4); // Colors for (int i = 0; i < 4; i++) { C.push_back(color); } // Normals N.push_back(c1.getNormalized()); N.push_back(c2.getNormalized()); N.push_back(c3.getNormalized()); N.push_back(c4.getNormalized()); const UINT baseIdx = static_cast<UINT>(t * slices * 4 + p * 4); // Indices std::vector<unsigned int> indices; if ( t == 0 ) { // top cap -- t1p1, t2p2, t2p1 indices.push_back(baseIdx + 0); indices.push_back(baseIdx + 2); indices.push_back(baseIdx + 3); I.push_back(indices); } else if ( t + 1 == stacks ) { // bottom cap -- t2p2, t1p1, t1p2 indices.push_back(baseIdx + 2); indices.push_back(baseIdx + 0); indices.push_back(baseIdx + 1); I.push_back(indices); } else { // regular piece indices.push_back(baseIdx + 0); indices.push_back(baseIdx + 1); indices.push_back(baseIdx + 3); I.push_back(indices); indices.clear(); indices.push_back(baseIdx + 1); indices.push_back(baseIdx + 2); indices.push_back(baseIdx + 3); I.push_back(indices); } } } //meshdata.mergeCloseVertices(0.00001f, true); return TriMeshf(meshdata); }
void GeometryGenerator::CreateCylinder(float bottomRadius, float topRadius, float height, UINT sliceCount, UINT stackCount, MeshData& meshData) { meshData.Vertices.clear(); meshData.Indices.clear(); // // Build Stacks. // float stackHeight = height / stackCount; // Amount to increment radius as we move up each stack level from bottom to top. float radiusStep = (topRadius - bottomRadius) / stackCount; UINT ringCount = stackCount + 1; // Compute vertices for each stack ring starting at the bottom and moving up. for (UINT i = 0; i < ringCount; ++i) { float y = -0.5f*height + i*stackHeight; float r = bottomRadius + i*radiusStep; // vertices of ring float dTheta = 2.0f*XM_PI / sliceCount; for (UINT j = 0; j <= sliceCount; ++j) { Vertex vertex; float c = cosf(j*dTheta); float s = sinf(j*dTheta); vertex.Position = XMFLOAT3(r*c, y, r*s); vertex.TexC.x = (float)j / sliceCount; vertex.TexC.y = 1.0f - (float)i / stackCount; // Cylinder can be parameterized as follows, where we introduce v // parameter that goes in the same direction as the v tex-coord // so that the bitangent goes in the same direction as the v tex-coord. // Let r0 be the bottom radius and let r1 be the top radius. // y(v) = h - hv for v in [0,1]. // r(v) = r1 + (r0-r1)v // // x(t, v) = r(v)*cos(t) // y(t, v) = h - hv // z(t, v) = r(v)*sin(t) // // dx/dt = -r(v)*sin(t) // dy/dt = 0 // dz/dt = +r(v)*cos(t) // // dx/dv = (r0-r1)*cos(t) // dy/dv = -h // dz/dv = (r0-r1)*sin(t) // This is unit length. vertex.TangentU = XMFLOAT3(-s, 0.0f, c); float dr = bottomRadius - topRadius; XMFLOAT3 bitangent(dr*c, -height, dr*s); XMVECTOR T = XMLoadFloat3(&vertex.TangentU); XMVECTOR B = XMLoadFloat3(&bitangent); XMVECTOR N = XMVector3Normalize(XMVector3Cross(T, B)); XMStoreFloat3(&vertex.Normal, N); meshData.Vertices.push_back(vertex); } } // Add one because we duplicate the first and last vertex per ring // since the texture coordinates are different. UINT ringVertexCount = sliceCount + 1; // Compute indices for each stack. for (UINT i = 0; i < stackCount; ++i) { for (UINT j = 0; j < sliceCount; ++j) { meshData.Indices.push_back(i*ringVertexCount + j); meshData.Indices.push_back((i + 1)*ringVertexCount + j); meshData.Indices.push_back((i + 1)*ringVertexCount + j + 1); meshData.Indices.push_back(i*ringVertexCount + j); meshData.Indices.push_back((i + 1)*ringVertexCount + j + 1); meshData.Indices.push_back(i*ringVertexCount + j + 1); } } BuildCylinderTopCap(bottomRadius, topRadius, height, sliceCount, stackCount, meshData); BuildCylinderBottomCap(bottomRadius, topRadius, height, sliceCount, stackCount, meshData); }
float SinCos(float x, float y) { return 0.3f * cosf(3.0f * x * PI) * sinf(3.0f * y * PI); }
VECTOR4D SinCosNormal(float x, float y, float z) { VECTOR4D N = { 0.3f * 3.0f * PI * sin(3.0f * x * PI) * sinf(3.0f * y * PI), -0.3f * 3.0f * PI * cosf(3.0f * x * PI) * cos(3.0f * y * PI), 1.0f, 0.0f }; N = Normalize(N); return N; }
void PVScene::draw(int x, int y, int w, int h) { static float time = 0.0f; time += 0.01f; viewMatrix = lookAtLH(float3(cosf(time) * 5, 2, sinf(time) * 5), float3(0, 0, 0), float3(0, 1, 0)); float4x4 viewProjectionMatrix = projectionMatrix * viewMatrix; glClearColor(0.0f, 0.0f, 0.0f, 1.0f); glClearDepth(1.0f); glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glBindVertexArray(vertexArray); shader->bind(); GLuint viewProjectionLocation = shader->getUniformLocation("viewProjection"); glUniformMatrix4fv(viewProjectionLocation, 1, true, &viewProjectionMatrix.m[0][0]); GLuint lightColorLocation = shader->getUniformLocation("lightColor"); GLuint lightPositionLocation = shader->getUniformLocation("lightPosition"); GLuint numLightsLocation = shader->getUniformLocation("numLights"); float lightPositions[3 * 4]; float lightColors[3 * 4]; int numLights = lights.size(); if (numLights > 4) numLights = 4; for (int i = 0; i < numLights; i++) { PVLight *light = lights[i]; float3 lightPosition = light->getPosition(); float3 lightColor = light->getColor(); lightPositions[i * 3 + 0] = lightPosition.x; lightPositions[i * 3 + 1] = lightPosition.y; lightPositions[i * 3 + 2] = lightPosition.z; lightColors[i * 3 + 0] = lightColor.x; lightColors[i * 3 + 1] = lightColor.y; lightColors[i * 3 + 2] = lightColor.z; } glUniform3fv(lightPositionLocation, numLights, lightPositions); glUniform3fv(lightColorLocation, numLights, lightColors); glUniform1i(numLightsLocation, numLights); glEnable(GL_DEPTH_TEST); glEnable(GL_CULL_FACE); glCullFace(GL_BACK); glFrontFace(GL_CW); glViewport(x, y, w, h); for (PVMesh *mesh : meshes) mesh->draw(shader); glDisable(GL_CULL_FACE); glActiveTexture(GL_TEXTURE0); glDisable(GL_DEPTH_TEST); glUseProgram(0); glBindVertexArray(0); GLCHECK(); }
bool AudioOutput::mix(void *outbuff, unsigned int nsamp) { QList<AudioOutputUser *> qlMix; QList<AudioOutputUser *> qlDel; if (g.s.fVolume < 0.01f) { return false; } const float adjustFactor = std::pow(10.f , -18.f / 20); const float mul = g.s.fVolume; const unsigned int nchan = iChannels; ServerHandlerPtr sh = g.sh; VoiceRecorderPtr recorder; if (sh) { recorder = g.sh->recorder; } qrwlOutputs.lockForRead(); bool prioritySpeakerActive = false; QMultiHash<const ClientUser *, AudioOutputUser *>::const_iterator it = qmOutputs.constBegin(); while (it != qmOutputs.constEnd()) { AudioOutputUser *aop = it.value(); if (! aop->needSamples(nsamp)) { qlDel.append(aop); } else { qlMix.append(aop); const ClientUser *user = it.key(); if (user && user->bPrioritySpeaker) { prioritySpeakerActive = true; } } ++it; } if (g.prioritySpeakerActiveOverride) { prioritySpeakerActive = true; } if (! qlMix.isEmpty()) { STACKVAR(float, speaker, iChannels*3); STACKVAR(float, svol, iChannels); STACKVAR(float, fOutput, iChannels * nsamp); float *output = (eSampleFormat == SampleFloat) ? reinterpret_cast<float *>(outbuff) : fOutput; bool validListener = false; memset(output, 0, sizeof(float) * nsamp * iChannels); boost::shared_array<float> recbuff; if (recorder) { recbuff = boost::shared_array<float>(new float[nsamp]); memset(recbuff.get(), 0, sizeof(float) * nsamp); recorder->prepareBufferAdds(); } for (unsigned int i=0;i<iChannels;++i) svol[i] = mul * fSpeakerVolume[i]; if (g.s.bPositionalAudio && (iChannels > 1) && g.p->fetch() && (g.bPosTest || g.p->fCameraPosition[0] != 0 || g.p->fCameraPosition[1] != 0 || g.p->fCameraPosition[2] != 0)) { float front[3] = { g.p->fCameraFront[0], g.p->fCameraFront[1], g.p->fCameraFront[2] }; float top[3] = { g.p->fCameraTop[0], g.p->fCameraTop[1], g.p->fCameraTop[2] }; // Front vector is dominant; if it's zero we presume all is zero. float flen = sqrtf(front[0]*front[0]+front[1]*front[1]+front[2]*front[2]); if (flen > 0.0f) { front[0] *= (1.0f / flen); front[1] *= (1.0f / flen); front[2] *= (1.0f / flen); float tlen = sqrtf(top[0]*top[0]+top[1]*top[1]+top[2]*top[2]); if (tlen > 0.0f) { top[0] *= (1.0f / tlen); top[1] *= (1.0f / tlen); top[2] *= (1.0f / tlen); } else { top[0] = 0.0f; top[1] = 1.0f; top[2] = 0.0f; } if (std::abs<float>(front[0] * top[0] + front[1] * top[1] + front[2] * top[2]) > 0.01f) { // Not perpendicular. Assume Y up and rotate 90 degrees. float azimuth = 0.0f; if ((front[0] != 0.0f) || (front[2] != 0.0f)) azimuth = atan2f(front[2], front[0]); float inclination = acosf(front[1]) - static_cast<float>(M_PI) / 2.0f; top[0] = sinf(inclination)*cosf(azimuth); top[1] = cosf(inclination); top[2] = sinf(inclination)*sinf(azimuth); } } else { front[0] = 0.0f; front[1] = 0.0f; front[2] = 1.0f; top[0] = 0.0f; top[1] = 1.0f; top[2] = 0.0f; } // Calculate right vector as front X top float right[3] = {top[1]*front[2] - top[2]*front[1], top[2]*front[0] - top[0]*front[2], top[0]*front[1] - top[1] * front[0] }; /* qWarning("Front: %f %f %f", front[0], front[1], front[2]); qWarning("Top: %f %f %f", top[0], top[1], top[2]); qWarning("Right: %f %f %f", right[0], right[1], right[2]); */ // Rotate speakers to match orientation for (unsigned int i=0;i<iChannels;++i) { speaker[3*i+0] = fSpeakers[3*i+0] * right[0] + fSpeakers[3*i+1] * top[0] + fSpeakers[3*i+2] * front[0]; speaker[3*i+1] = fSpeakers[3*i+0] * right[1] + fSpeakers[3*i+1] * top[1] + fSpeakers[3*i+2] * front[1]; speaker[3*i+2] = fSpeakers[3*i+0] * right[2] + fSpeakers[3*i+1] * top[2] + fSpeakers[3*i+2] * front[2]; } validListener = true; } foreach(AudioOutputUser *aop, qlMix) { const float * RESTRICT pfBuffer = aop->pfBuffer; float volumeAdjustment = 1; if (prioritySpeakerActive) { AudioOutputSpeech *speech = qobject_cast<AudioOutputSpeech *>(aop); if (speech) { const ClientUser* user = speech->p; if (user->tsState != Settings::Whispering && !user->bPrioritySpeaker) { volumeAdjustment = adjustFactor; } } } if (recorder) { AudioOutputSpeech *aos = qobject_cast<AudioOutputSpeech *>(aop); if (aos) { for (unsigned int i = 0; i < nsamp; ++i) { recbuff[i] += pfBuffer[i] * volumeAdjustment; } if (!recorder->isInMixDownMode()) { if (aos) { recorder->addBuffer(aos->p, recbuff, nsamp); } else { // this should be unreachable Q_ASSERT(false); } recbuff = boost::shared_array<float>(new float[nsamp]); memset(recbuff.get(), 0, sizeof(float) * nsamp); } // Don't add the local audio to the real output if (qobject_cast<RecordUser *>(aos->p)) { continue; } } } if (validListener && ((aop->fPos[0] != 0.0f) || (aop->fPos[1] != 0.0f) || (aop->fPos[2] != 0.0f))) { float dir[3] = { aop->fPos[0] - g.p->fCameraPosition[0], aop->fPos[1] - g.p->fCameraPosition[1], aop->fPos[2] - g.p->fCameraPosition[2] }; float len = sqrtf(dir[0] * dir[0] + dir[1] * dir[1] + dir[2] * dir[2]); if (len > 0.0f) { dir[0] /= len; dir[1] /= len; dir[2] /= len; } /* qWarning("Voice pos: %f %f %f", aop->fPos[0], aop->fPos[1], aop->fPos[2]); qWarning("Voice dir: %f %f %f", dir[0], dir[1], dir[2]); */ if (! aop->pfVolume) { aop->pfVolume = new float[nchan]; for (unsigned int s=0;s<nchan;++s) aop->pfVolume[s] = -1.0; } for (unsigned int s=0;s<nchan;++s) { const float dot = bSpeakerPositional[s] ? dir[0] * speaker[s*3+0] + dir[1] * speaker[s*3+1] + dir[2] * speaker[s*3+2] : 1.0f; const float str = svol[s] * calcGain(dot, len) * volumeAdjustment; float * RESTRICT o = output + s; const float old = (aop->pfVolume[s] >= 0.0f) ? aop->pfVolume[s] : str; const float inc = (str - old) / static_cast<float>(nsamp); aop->pfVolume[s] = str; /* qWarning("%d: Pos %f %f %f : Dot %f Len %f Str %f", s, speaker[s*3+0], speaker[s*3+1], speaker[s*3+2], dot, len, str); */ if ((old >= 0.00000001f) || (str >= 0.00000001f)) for (unsigned int i=0;i<nsamp;++i) o[i*nchan] += pfBuffer[i] * (old + inc*static_cast<float>(i)); } } else { for (unsigned int s=0;s<nchan;++s) {
void GL_draw_frame( void * vp ) { static float rot; static int t0; float s; glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glLineWidth(1.f); set_camera_view(); if ( t0 == 0 ) { init_VBO(); t0 = 1; } int now = get_milliseconds(); /* while ( now - t0 > 20 ) { rot += 1.0f; t0 += 20; if ( rot > 360.f ) rot -= 360.f; if ( rot < 0.f ) rot += 360.f; } */ // hack to get rotation timer right const float p180 = 0.017453126f; const float c180p = 57.295776f; const float c2pi = 6.2831854f; const int ms_per_frame = 20; // milliseconds per frame const float rot_per_frame = 1.0f * p180; /* while ( now - t0 > ms_per_frame ) { rot += 1.0f; t0 += ms_per_frame; } */ static int set = 0; if ( t0 <= 0 ) t0 = 1; if ( now - t0 > 0 ) { int diff = now - t0; /* the rotation is incremented 1 rot_per_frame each 1 ms_per_frame */ float newrot = rot + (rot_per_frame/(float)ms_per_frame) * ((float)diff); if ( set < 20 ) core.printf( "hiccup > 2pi: before: %f, %f ", rot, sinf(rot) ); rot = newrot; // catch it up t0 = now; if ( set < 20 ) core.printf( "after: %f, %f\n", rot, sinf( rot ) ); // clamp if ( rot > c2pi ) { rot = rot - c2pi; set = 1; // no more print core.printf( " --> MARK <-- \n" ); } if ( set != 0 ) ++set; } const float rotdeg = rot * c180p; /// -- DRAW --- // rotating wire spiral glColor4ub( 255,255,255,255 ); glPushMatrix(); glRotatef( rotdeg, 0, 1, 0 ); draw_spiral( 24.0f, 10.f, 0.05f ); glPopMatrix(); // rotating circle glEnable(GL_TEXTURE_2D); glBindTexture( GL_TEXTURE_2D, core.materials.findByName( "jr_bob" )->img->texhandle ); glPushMatrix(); glTranslatef( 60.f, 60.f, -60.f ); glRotatef( rotdeg, 0, 1, 0 ); draw_circle( 0, 0, 10.f ); GL_TEX_SQUARE( -10.f, 30.f, 0.f, 20.f ); glRotatef( 180, 0, 1, 0 ); draw_circle( 0, 0, 10.f ); GL_TEX_SQUARE( -10.f, 30.f, 0.f, 20.f ); glBegin(GL_LINES); glVertex3f( 0, -30.f, 0 ); glVertex3f( 0, 30.f, 0 ); glEnd(); glPopMatrix(); glDisable( GL_TEXTURE_2D ); // FIXME : obviously move, later. tessellatedPlane_t& tes = *(tessellatedPlane_t*)vp; // heightmap triangles glInterleavedArrays( GL_C3F_V3F, 0, tes.array ); glDrawArrays( GL_TRIANGLE_STRIP, 0, tes.num_verts ); //glDrawArrays( GL_LINE_STRIP, 0, tes->num_verts ); // gazillion boxes core.drawer.drawLists(); // serpinski glColor4ub( 255,255,0,255 ); glPushMatrix(); glTranslatef( 250.f, 100.f, -250.f ); glRotatef( rotdeg * 1.618f, 0, 1, 0 ); glRotatef( rotdeg , 0.707, 0, -0.707 ); glDisable( GL_CULL_FACE ); draw_serpinski( 3, 40.f ); glEnable( GL_CULL_FACE ); glPopMatrix(); glColor4ub( 255,255,255,255 ); // icosahedron glPushMatrix(); glTranslatef( 500.f, 100.f, -250.f ); trans_spiral_f( rot, 50.f /*radius*/, 1.0f/*rot rate*/, 1.0f/*climb rate*/, 50.f /*ceiling*/ ); s = 14.f; s = (sinf( rot * 2.718f ) + 1.1f) * 14.f; glScalef( s, s, s ); glRotatef( rotdeg * 5.f, 0, 1, 0 ); glRotatef( rotdeg * 3.f , 0.707, 0, -0.707 ); glColor4ub( 230, 100, 0, 255 ); draw_icosahedron(); glColor4ub( 255, 255, 255, 255 ); draw_icosahedron_wire(); glPopMatrix(); // axis marker float l = 100.f; glColor3f( 1.f, 0.f, 1.f ); glLineWidth(2.f); glBegin( GL_LINES ); glVertex3i( 0,0,0 ); glVertex3i( 0,l,0); glVertex3i( 0,0,0 ); glVertex3i( l,0,0 ); glVertex3i( 0,0,0 ); glVertex3i( 0,0,-l ); glEnd(); // 4-sided glPushMatrix(); glTranslatef( 300, 100, 0 ); s = 20.f; glScalef( s, s, s ); glRotatef( rotdeg, 0, 1, 0 ); glColor4ub( 0, 255, 255, 128 ); draw_triangle4(0); glColor4ub( 255, 255, 255, 255 ); draw_triangle4(1); glPopMatrix(); // 4-sided, 2nd type glPushMatrix(); glTranslatef( 340, 100, 0 ); s = 20.f; glScalef( s, s, s ); glRotatef( rotdeg, 0, 1, 0 ); glColor4ub( 100, 0, 100, 128 ); draw_triangle4_2(0); glColor4ub( 255, 255, 255, 255 ); draw_triangle4(1); // inner lines don't draw right, so use first form glPopMatrix(); // 5-sided glPushMatrix(); glTranslatef( 100, 100, -50 ); s = 20.f; glScalef( s, s, s ); glRotatef( rotdeg, 1, 0, 0 ); glColor4ub( 100, 50, 0, 200 ); draw_triangle5(0); glColor4ub( 255, 255, 255, 255 ); draw_triangle5(1); glPopMatrix(); // unit-cube w/ tri glPushMatrix(); glTranslatef( 150.f, 130.f, -800.f ); glTranslatef( 0.f, 0.f, sinf(rot)*1600.f ); s = 20.f; glScalef( s, s, s ); glRotatef( 90, 1, 0, 0 ); glRotatef( sinf(rotdeg*0.03f)*180.f, 0, 1, 0 ); glColor4ub( 128,128,128,255 ); draw_unitcube(0); glColor4ub( 255,255,255,255 ); draw_unitcube(1); glTranslatef( 0, 1.f, 0.f ); glColor4ub( 128,128,128,255 ); draw_triangle5(0); glColor4ub( 255,255,255,255 ); draw_triangle5(1); glPopMatrix(); // test model //glEnable(GL_LIGHTING); glPushMatrix(); glTranslatef( 300.f, 75.f, 200.f ); glRotatef( 315.f, 0,1,0 ); glEnable(GL_LIGHT0); s = 550.f; glScalef( s, s, s ); glColor4ub( 255,255,255,255); //draw_test_model_Vertex_Arrays(); draw_test_model_VBO(); glPolygonMode(GL_FRONT_AND_BACK, GL_LINE); glColor4ub( 128,128,168,255 ); //draw_test_model_Vertex_Arrays(); draw_test_model_VBO(); glPolygonMode(GL_FRONT_AND_BACK, GL_FILL); glPopMatrix(); glDisable(GL_LIGHTING); glDisable(GL_LIGHT0); glFlush(); SDL_GL_SwapBuffers(); }
void CCEaseSineOut::update(float time) { m_pOther->update(sinf(time * (float)M_PI_2)); }
void synthesise( kiss_fft_cfg fft_inv_cfg, float Sn_[], /* time domain synthesised signal */ MODEL *model, /* ptr to model parameters for this frame */ float Pn[], /* time domain Parzen window */ int shift /* flag used to handle transition frames */ ) { int i,l,j,b; /* loop variables */ COMP Sw_[FFT_DEC]; /* DFT of synthesised signal */ COMP sw_[FFT_DEC]; /* synthesised signal */ if (shift) { /* Update memories */ for(i=0; i<N-1; i++) { Sn_[i] = Sn_[i+N]; } Sn_[N-1] = 0.0; } for(i=0; i<FFT_DEC; i++) { Sw_[i].real = 0.0; Sw_[i].imag = 0.0; } /* Nov 2010 - found that synthesis using time domain cos() functions gives better results for synthesis frames greater than 10ms. Inverse FFT synthesis using a 512 pt FFT works well for 10ms window. I think (but am not sure) that the problem is related to the quantisation of the harmonic frequencies to the FFT bin size, e.g. there is a 8000/512 Hz step between FFT bins. For some reason this makes the speech from longer frame > 10ms sound poor. The effect can also be seen when synthesising test signals like single sine waves, some sort of amplitude modulation at the frame rate. Another possibility is using a larger FFT size (1024 or 2048). */ #define FFT_SYNTHESIS #ifdef FFT_SYNTHESIS /* Now set up frequency domain synthesised speech */ for(l=1; l<=model->L; l++) { //for(l=model->L/2; l<=model->L; l++) { //for(l=1; l<=model->L/4; l++) { b = (int)(l*model->Wo*FFT_DEC/TWO_PI + 0.5); if (b > ((FFT_DEC/2)-1)) { b = (FFT_DEC/2)-1; } Sw_[b].real = model->A[l]*cosf(model->phi[l]); Sw_[b].imag = model->A[l]*sinf(model->phi[l]); Sw_[FFT_DEC-b].real = Sw_[b].real; Sw_[FFT_DEC-b].imag = -Sw_[b].imag; } /* Perform inverse DFT */ kiss_fft(fft_inv_cfg, (kiss_fft_cpx *)Sw_, (kiss_fft_cpx *)sw_); #else /* Direct time domain synthesis using the cos() function. Works well at 10ms and 20ms frames rates. Note synthesis window is still used to handle overlap-add between adjacent frames. This could be simplified as we don't need to synthesise where Pn[] is zero. */ for(l=1; l<=model->L; l++) { for(i=0,j=-N+1; i<N-1; i++,j++) { Sw_[FFT_DEC-N+1+i].real += 2.0*model->A[l]*cos(j*model->Wo*l + model->phi[l]); } for(i=N-1,j=0; i<2*N; i++,j++) Sw_[j].real += 2.0*model->A[l]*cos(j*model->Wo*l + model->phi[l]); } #endif /* Overlap add to previous samples */ for(i=0; i<N-1; i++) { Sn_[i] += sw_[FFT_DEC-N+1+i].real*Pn[i]; } if (shift) for(i=N-1,j=0; i<2*N; i++,j++) Sn_[i] = sw_[j].real*Pn[i]; else for(i=N-1,j=0; i<2*N; i++,j++) Sn_[i] += sw_[j].real*Pn[i]; }
void getOrientation(float *smoothAcc, float *orient, float *accData, float *gyroData, float dt) { float accAngle[3]; float gyroRate[3]; if (evvgcCFinitialized == false) { initOrientation(); evvgcCFinitialized = true; } //------------------------------------------- if (evvgcCFinitialized == true) { accAngle[ROLL ] = atan2f(-accData[YAXIS], -accData[ZAXIS]); accAngle[PITCH] = atan2f(accData[XAXIS], -accData[ZAXIS]); smoothAcc[ROLL] = ((smoothAcc[ROLL ] * 99.0f) + accAngle[ROLL ]) / 100.0f; smoothAcc[PITCH] = ((smoothAcc[PITCH] * 99.0f) + accAngle[PITCH]) / 100.0f; gyroRate[PITCH] = gyroData[PITCH]; orient[PITCH] = (orient[PITCH] + gyroRate[PITCH] * dt) + 0.0002f * (smoothAcc[PITCH] - orient[PITCH]); gyroRate[ROLL] = gyroData[ROLL] * cosf(fabsf(orient[PITCH])) + gyroData[YAW] * sinf(orient[PITCH]); orient[ROLL] = (orient[ROLL] + gyroRate[ROLL] * dt) + 0.0002f * (smoothAcc[ROLL] - orient[ROLL]); gyroRate[YAW] = gyroData[YAW] * cosf(fabsf(orient[PITCH])) - gyroData[ROLL] * sinf(orient[PITCH]); orient[YAW] = (orient[YAW] + gyroRate[YAW] * dt); } }
void stabilisation_wing_cascade_stabilise(stabilisation_wing_t* stabilisation_wing) { float rpyt_errors[4]; control_command_t input; int32_t i; float feedforward[4]; float nav_heading, current_heading, heading_error; float gps_speed_global[3], gps_speed_semi_local[3]; float input_turn_rate; float input_roll_angle; aero_attitude_t attitude, attitude_yaw; quat_t q_rot; float airspeed_desired; float clipping_factor; // Update timing float now = time_keeper_get_s(); stabilisation_wing->dt_s = now - stabilisation_wing->last_update_s; stabilisation_wing->last_update_s = now; // Get up vector in body frame quat_t up = {0.0f, {UPVECTOR_X, UPVECTOR_Y, UPVECTOR_Z}}; quat_t up_vec = quaternions_global_to_local(stabilisation_wing->ahrs->qe, up); // set the controller input input= *stabilisation_wing->controls; switch (stabilisation_wing->controls->control_mode) { case VELOCITY_COMMAND_MODE: // Get current attitude attitude_yaw = coord_conventions_quat_to_aero(stabilisation_wing->ahrs->qe); ///////////// // HEADING // ///////////// // Compute the heading angle corresponding to the given input velocity vector (input from remote/vector field should be in semi-local frame). nav_heading = heading_from_velocity_vector(input.tvel); // Overwrite command if in remote if(stabilisation_wing->controls->yaw_mode == YAW_RELATIVE) { nav_heading = maths_calc_smaller_angle(input.rpy[YAW] - attitude_yaw.rpy[YAW]); } // Compute current heading gps_speed_global[X] = stabilisation_wing->ins->velocity_lf()[0]; gps_speed_global[Y] = stabilisation_wing->ins->velocity_lf()[1]; gps_speed_global[Z] = stabilisation_wing->ins->velocity_lf()[Z]; // Transform global to semi-local attitude_yaw.rpy[0] = 0.0f; attitude_yaw.rpy[1] = 0.0f; attitude_yaw.rpy[2] = -attitude_yaw.rpy[2]; q_rot = coord_conventions_quaternion_from_aero(attitude_yaw); quaternions_rotate_vector(q_rot, gps_speed_global, gps_speed_semi_local); current_heading = heading_from_velocity_vector(gps_speed_semi_local); // Compute heading error heading_error = maths_calc_smaller_angle(nav_heading - current_heading); /////////////// // PID INPUT // /////////////// // Vector field normalize vector in plane x-y to cruise_speed value ==> airspeed should be done only on the x-y composants airspeed_desired = sqrtf(input.tvel[X]*input.tvel[X] + input.tvel[Y]*input.tvel[Y]); // Compute errors rpyt_errors[0] = heading_error; // Heading rpyt_errors[1] = input.tvel[Z] - gps_speed_global[Z]; // Vertical speed rpyt_errors[2] = 0.0f; rpyt_errors[3] = airspeed_desired - stabilisation_wing->airspeed_analog->get_airspeed(); // Airspeed // Compute the feedforward feedforward[0] = 0.0f; feedforward[1] = 0.0f; feedforward[2] = 0.0f; feedforward[3] = (airspeed_desired - 13.0f)/8.0f + 0.2f; // run PID update on all velocity controllers stabilisation_run_feedforward(&stabilisation_wing->stabiliser_stack.velocity_stabiliser, stabilisation_wing->dt_s, rpyt_errors, feedforward); //////////////// // PID OUTPUT // //////////////// // Get turn rate command and transform it into a roll angle command for next layer input_turn_rate = stabilisation_wing->stabiliser_stack.velocity_stabiliser.output.rpy[0]; // TODO: Fix this in case of bad airspeed readings... clipping_factor = stabilisation_wing->max_roll_angle / (PI/2.0f); if(clipping_factor == 0.0f) { input_roll_angle = 0.0f; } else { input_roll_angle = clipping_factor * atanf( (1.0f/clipping_factor) * (stabilisation_wing->airspeed_analog->get_airspeed() * input_turn_rate / 9.81f) ); } // Set input for next layer input = stabilisation_wing->stabiliser_stack.velocity_stabiliser.output; input.rpy[0] = input_roll_angle; input.rpy[1] = - stabilisation_wing->stabiliser_stack.velocity_stabiliser.output.rpy[1]; input.thrust = stabilisation_wing->stabiliser_stack.velocity_stabiliser.output.thrust; // Overwrite the commands during different key phases (take-off and landing) if(stabilisation_wing->navigation->internal_state() == Navigation::NAV_TAKEOFF) { // Take-off: fixed 0 roll angle, fixed defined pitch angle and fixed defined constant thrust value. input.rpy[0] = 0.0f; input.rpy[1] = stabilisation_wing->take_off_pitch; input.thrust = stabilisation_wing->take_off_thrust; } else if(stabilisation_wing->navigation->internal_state() == Navigation::NAV_LANDING) { // Landing: Limit the roll computed by the velocity layer (navigation), shut down the motor and impose a little pitch down to assure gliding without stall. if(input.rpy[0] > stabilisation_wing->landing_max_roll) { input.rpy[0] = stabilisation_wing->landing_max_roll; } else if(input.rpy[0] < -stabilisation_wing->landing_max_roll) { input.rpy[0] = -stabilisation_wing->landing_max_roll; } input.rpy[1] = stabilisation_wing->landing_pitch; input.thrust = -1.0f; } // -- no break here - we want to run the lower level modes as well! -- case ATTITUDE_COMMAND_MODE: // Add "a priori on the pitch" to fly horizontally and to compensate for roll angle attitude = coord_conventions_quat_to_aero(stabilisation_wing->ahrs->qe); input.rpy[1] += stabilisation_wing->pitch_angle_apriori; // Constant compensation for horizontal if(maths_f_abs(attitude.rpy[ROLL]) < PI/2.0f) // Compensation for the roll bank angle { input.rpy[1] += stabilisation_wing->pitch_angle_apriori_gain * maths_f_abs(input.rpy[0]*input.rpy[0]*input.rpy[0]); } // run absolute attitude_filter controller rpyt_errors[0]= sinf(input.rpy[0]) + up_vec.v[1]; // Roll rpyt_errors[1]= sinf(input.rpy[1]) - up_vec.v[0]; // Pitch rpyt_errors[2]= 0.0f; // Yaw rpyt_errors[3]= input.thrust; // no feedback for thrust at this level // run PID update on all attitude_filter controllers stabilisation_run(&stabilisation_wing->stabiliser_stack.attitude_stabiliser, stabilisation_wing->dt_s, rpyt_errors); // use output of attitude_filter controller to set rate setpoints for rate controller input = stabilisation_wing->stabiliser_stack.attitude_stabiliser.output; // -- no break here - we want to run the lower level modes as well! -- case RATE_COMMAND_MODE: // this level is always run // get rate measurements from IMU (filtered angular rates) for (i=0; i<3; i++) { rpyt_errors[i]= input.rpy[i]- stabilisation_wing->ahrs->angular_speed[i]; } rpyt_errors[3] = input.thrust ; // no feedback for thrust at this level // run PID update on all rate controllers stabilisation_run(&stabilisation_wing->stabiliser_stack.rate_stabiliser, stabilisation_wing->dt_s, rpyt_errors); } stabilisation_wing->torque_command->xyz[0] = stabilisation_wing->stabiliser_stack.rate_stabiliser.output.rpy[ROLL]; stabilisation_wing->torque_command->xyz[1] = stabilisation_wing->stabiliser_stack.rate_stabiliser.output.rpy[PITCH]; stabilisation_wing->thrust_command->thrust = stabilisation_wing->stabiliser_stack.rate_stabiliser.output.thrust; }
int gl_renderer::render() { int running = 1; /* ----- Event cycle --------------- */ while (SDL_PollEvent(&event)) { switch (event.type) { case SDL_QUIT: running = 0; break; case SDL_KEYDOWN: switch( event.key.keysym.sym ){ case SDLK_ESCAPE: case SDLK_q: running = 0; break; case SDLK_F1: SDL_WM_ToggleFullScreen(screen); break; case SDLK_s: control1->toggleEngineL(); break; case SDLK_d: control1->toggleEngineR(); break; case SDLK_k: control2->toggleEngineL(); break; case SDLK_l: control2->toggleEngineR(); break; case SDLK_1: switch_camera(0); break; case SDLK_2: switch_camera(1); break; case SDLK_3: switch_camera(2); break; default: break; } break; case SDL_VIDEORESIZE: if((screen = SDL_SetVideoMode(event.resize.w, event.resize.h, bpp, flags)) == 0) { fprintf(stderr, "Video resize failed: %s\n", SDL_GetError()); exit(-1); } glPushAttrib(GL_TRANSFORM_BIT); glMatrixMode(GL_PROJECTION); glLoadIdentity(); gluPerspective(90.0f, (GLfloat)event.resize.w/(GLfloat)event.resize.h, 0.1, 100000.0); glViewport(0.0f, 0.0f, event.resize.w, event.resize.h); glPopAttrib(); break; case SDL_ACTIVEEVENT: if(event.active.state != SDL_APPMOUSEFOCUS && event.active.gain == 0) while(1){ SDL_WaitEvent(&event); if(event.type == SDL_ACTIVEEVENT && event.active.state != SDL_APPMOUSEFOCUS && event.active.gain == 1); break; } break; default: break; } } interval = FrameTiming(); /* apply control movement */ /* ----- Blitting on the screen --------------- */ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT); glLoadIdentity(); current_camera->apply_rotation(); //glutSolidCube(50.0); //draw_skybox(200.0); background_stars->draw(); current_camera->apply_translation(); /* ----- Light ----- */ angle += 0.001f * interval; light1.Position[0] = sinf(angle) * 1.5f; light1.Position[2] = cosf(angle) * 1.5f; light2.Position[1] = sinf(angle) * 1.5f; light2.Position[2] = cosf(angle) * 1.5f; UpdateLight(&light1, GL_LIGHT1, 0.1f); UpdateLight(&light2, GL_LIGHT2, 0.1f); /* ----- Objects ----- */ for(list<gl_drawable*>::const_iterator it = drawables->begin(); it != drawables->end(); ++it) { (*it)->draw(); } SDL_GL_SwapBuffers(); /* SDL_Delay(25); */ /* Decomment this if you want 1/50th screen update */ return running; }
void CCharacter::FireWeapon() { if(m_ReloadTimer != 0) return; /* INFECTION MODIFICATION START ***************************************/ if(GetClass() == PLAYERCLASS_NONE) return; /* INFECTION MODIFICATION END *****************************************/ DoWeaponSwitch(); vec2 Direction = normalize(vec2(m_LatestInput.m_TargetX, m_LatestInput.m_TargetY)); bool FullAuto = false; if(m_ActiveWeapon == WEAPON_GRENADE || m_ActiveWeapon == WEAPON_SHOTGUN || m_ActiveWeapon == WEAPON_RIFLE) FullAuto = true; // check if we gonna fire bool WillFire = false; if(CountInput(m_LatestPrevInput.m_Fire, m_LatestInput.m_Fire).m_Presses) WillFire = true; if(FullAuto && (m_LatestInput.m_Fire&1) && m_aWeapons[m_ActiveWeapon].m_Ammo) WillFire = true; if(!WillFire) return; // check for ammo if(!m_aWeapons[m_ActiveWeapon].m_Ammo) { // 125ms is a magical limit of how fast a human can click m_ReloadTimer = 125 * Server()->TickSpeed() / 1000; if(m_LastNoAmmoSound+Server()->TickSpeed() <= Server()->Tick()) { GameServer()->CreateSound(m_Pos, SOUND_WEAPON_NOAMMO); m_LastNoAmmoSound = Server()->Tick(); } return; } vec2 ProjStartPos = m_Pos+Direction*m_ProximityRadius*0.75f; switch(m_ActiveWeapon) { case WEAPON_HAMMER: { /* INFECTION MODIFICATION START ***************************************/ if(GetClass() == PLAYERCLASS_ENGINEER) { if(m_pBarrier) { GameServer()->m_World.DestroyEntity(m_pBarrier); m_pBarrier = 0; } if(m_FirstShot) { m_FirstShot = false; m_FirstShotCoord = m_Pos; } else if(distance(m_FirstShotCoord, m_Pos) > 10.0) { m_FirstShot = true; CBarrier *pBarrier = new CBarrier(GameWorld(), m_FirstShotCoord, m_Pos, m_pPlayer->GetCID()); m_pBarrier = pBarrier; GameServer()->CreateSound(m_Pos, SOUND_RIFLE_FIRE); } } else if(GetClass() == PLAYERCLASS_SOLDIER) { if(m_pBomb) { m_pBomb->Explode(); } else { CBomb *pBomb = new CBomb(GameWorld(), ProjStartPos, m_pPlayer->GetCID()); m_pBomb = pBomb; GameServer()->CreateSound(m_Pos, SOUND_GRENADE_FIRE); } } else if(GetClass() == PLAYERCLASS_BOOMER) { Die(m_pPlayer->GetCID(), WEAPON_WORLD); } else { /* INFECTION MODIFICATION END *****************************************/ // reset objects Hit m_NumObjectsHit = 0; GameServer()->CreateSound(m_Pos, SOUND_HAMMER_FIRE); CCharacter *apEnts[MAX_CLIENTS]; int Hits = 0; int Num = GameServer()->m_World.FindEntities(ProjStartPos, m_ProximityRadius*0.5f, (CEntity**)apEnts, MAX_CLIENTS, CGameWorld::ENTTYPE_CHARACTER); for (int i = 0; i < Num; ++i) { CCharacter *pTarget = apEnts[i]; if ((pTarget == this) || GameServer()->Collision()->IntersectLine(ProjStartPos, pTarget->m_Pos, NULL, NULL)) continue; // set his velocity to fast upward (for now) if(length(pTarget->m_Pos-ProjStartPos) > 0.0f) GameServer()->CreateHammerHit(pTarget->m_Pos-normalize(pTarget->m_Pos-ProjStartPos)*m_ProximityRadius*0.5f); else GameServer()->CreateHammerHit(ProjStartPos); vec2 Dir; if (length(pTarget->m_Pos - m_Pos) > 0.0f) Dir = normalize(pTarget->m_Pos - m_Pos); else Dir = vec2(0.f, -1.f); /* INFECTION MODIFICATION START ***************************************/ if(IsInfected() && pTarget->IsInfected()) { pTarget->IncreaseHealth(2); pTarget->IncreaseArmor(2); pTarget->m_Core.m_Vel += vec2(0.f, -1.f) + normalize(Dir + vec2(0.f, -1.1f)) * 10.0f; pTarget->m_EmoteType = EMOTE_HAPPY; pTarget->m_EmoteStop = Server()->Tick() + Server()->TickSpeed(); } else { pTarget->TakeDamage(vec2(0.f, -1.f) + normalize(Dir + vec2(0.f, -1.1f)) * 10.0f, g_pData->m_Weapons.m_Hammer.m_pBase->m_Damage, m_pPlayer->GetCID(), m_ActiveWeapon); } /* INFECTION MODIFICATION END *****************************************/ Hits++; } // if we Hit anything, we have to wait for the reload if(Hits) m_ReloadTimer = Server()->TickSpeed()/3; /* INFECTION MODIFICATION START ***************************************/ } /* INFECTION MODIFICATION END *****************************************/ } break; case WEAPON_GUN: { CProjectile *pProj = new CProjectile(GameWorld(), WEAPON_GUN, m_pPlayer->GetCID(), ProjStartPos, Direction, (int)(Server()->TickSpeed()*GameServer()->Tuning()->m_GunLifetime), 1, 0, 0, -1, WEAPON_GUN); // pack the Projectile and send it to the client Directly CNetObj_Projectile p; pProj->FillInfo(&p); CMsgPacker Msg(NETMSGTYPE_SV_EXTRAPROJECTILE); Msg.AddInt(1); for(unsigned i = 0; i < sizeof(CNetObj_Projectile)/sizeof(int); i++) Msg.AddInt(((int *)&p)[i]); Server()->SendMsg(&Msg, 0, m_pPlayer->GetCID()); GameServer()->CreateSound(m_Pos, SOUND_GUN_FIRE); } break; case WEAPON_SHOTGUN: { int ShotSpread = 2; CMsgPacker Msg(NETMSGTYPE_SV_EXTRAPROJECTILE); Msg.AddInt(ShotSpread*2+1); for(int i = -ShotSpread; i <= ShotSpread; ++i) { float Spreading[] = {-0.185f, -0.070f, 0, 0.070f, 0.185f}; float a = GetAngle(Direction); a += Spreading[i+2]; float v = 1-(absolute(i)/(float)ShotSpread); float Speed = mix((float)GameServer()->Tuning()->m_ShotgunSpeeddiff, 1.0f, v); CProjectile *pProj = new CProjectile(GameWorld(), WEAPON_SHOTGUN, m_pPlayer->GetCID(), ProjStartPos, vec2(cosf(a), sinf(a))*Speed, (int)(Server()->TickSpeed()*GameServer()->Tuning()->m_ShotgunLifetime), 1, 0, 0, -1, WEAPON_SHOTGUN); // pack the Projectile and send it to the client Directly CNetObj_Projectile p; pProj->FillInfo(&p); for(unsigned i = 0; i < sizeof(CNetObj_Projectile)/sizeof(int); i++) Msg.AddInt(((int *)&p)[i]); } Server()->SendMsg(&Msg, 0,m_pPlayer->GetCID()); GameServer()->CreateSound(m_Pos, SOUND_SHOTGUN_FIRE); } break; case WEAPON_GRENADE: { CProjectile *pProj = new CProjectile(GameWorld(), WEAPON_GRENADE, m_pPlayer->GetCID(), ProjStartPos, Direction, (int)(Server()->TickSpeed()*GameServer()->Tuning()->m_GrenadeLifetime), 1, true, 0, SOUND_GRENADE_EXPLODE, WEAPON_GRENADE); // pack the Projectile and send it to the client Directly CNetObj_Projectile p; pProj->FillInfo(&p); CMsgPacker Msg(NETMSGTYPE_SV_EXTRAPROJECTILE); Msg.AddInt(1); for(unsigned i = 0; i < sizeof(CNetObj_Projectile)/sizeof(int); i++) Msg.AddInt(((int *)&p)[i]); Server()->SendMsg(&Msg, 0, m_pPlayer->GetCID()); GameServer()->CreateSound(m_Pos, SOUND_GRENADE_FIRE); } break; case WEAPON_RIFLE: { new CLaser(GameWorld(), m_Pos, Direction, GameServer()->Tuning()->m_LaserReach, m_pPlayer->GetCID()); GameServer()->CreateSound(m_Pos, SOUND_RIFLE_FIRE); } break; case WEAPON_NINJA: { // reset Hit objects m_NumObjectsHit = 0; m_Ninja.m_ActivationDir = Direction; m_Ninja.m_CurrentMoveTime = g_pData->m_Weapons.m_Ninja.m_Movetime * Server()->TickSpeed() / 1000; m_Ninja.m_OldVelAmount = length(m_Core.m_Vel); GameServer()->CreateSound(m_Pos, SOUND_NINJA_FIRE); } break; } m_AttackTick = Server()->Tick(); if(m_aWeapons[m_ActiveWeapon].m_Ammo > 0) // -1 == unlimited m_aWeapons[m_ActiveWeapon].m_Ammo--; if(!m_ReloadTimer) m_ReloadTimer = g_pData->m_Weapons.m_aId[m_ActiveWeapon].m_Firedelay * Server()->TickSpeed() / 1000; }
void GeometryGenerator::CreateGeosphere(float radius, UINT numSubdivisions, MeshData& meshData) { // Put a cap on the number of subdivisions. numSubdivisions = MathHelper::Min(numSubdivisions, 5u); // Approximate a sphere by tessellating an icosahedron. const float X = 0.525731f; const float Z = 0.850651f; XMFLOAT3 pos[12] = { XMFLOAT3(-X, 0.0f, Z), XMFLOAT3(X, 0.0f, Z), XMFLOAT3(-X, 0.0f, -Z), XMFLOAT3(X, 0.0f, -Z), XMFLOAT3(0.0f, Z, X), XMFLOAT3(0.0f, Z, -X), XMFLOAT3(0.0f, -Z, X), XMFLOAT3(0.0f, -Z, -X), XMFLOAT3(Z, X, 0.0f), XMFLOAT3(-Z, X, 0.0f), XMFLOAT3(Z, -X, 0.0f), XMFLOAT3(-Z, -X, 0.0f) }; DWORD k[60] = { 1,4,0, 4,9,0, 4,5,9, 8,5,4, 1,8,4, 1,10,8, 10,3,8, 8,3,5, 3,2,5, 3,7,2, 3,10,7, 10,6,7, 6,11,7, 6,0,11, 6,1,0, 10,1,6, 11,0,9, 2,11,9, 5,2,9, 11,2,7 }; meshData.Vertices.resize(12); meshData.Indices.resize(60); for (UINT i = 0; i < 12; ++i) meshData.Vertices[i].Position = pos[i]; for (UINT i = 0; i < 60; ++i) meshData.Indices[i] = k[i]; for (UINT i = 0; i < numSubdivisions; ++i) Subdivide(meshData); // Project vertices onto sphere and scale. for (UINT i = 0; i < meshData.Vertices.size(); ++i) { // Project onto unit sphere. XMVECTOR n = XMVector3Normalize(XMLoadFloat3(&meshData.Vertices[i].Position)); // Project onto sphere. XMVECTOR p = radius*n; XMStoreFloat3(&meshData.Vertices[i].Position, p); XMStoreFloat3(&meshData.Vertices[i].Normal, n); // Derive texture coordinates from spherical coordinates. float theta = MathHelper::AngleFromXY( meshData.Vertices[i].Position.x, meshData.Vertices[i].Position.z); float phi = acosf(meshData.Vertices[i].Position.y / radius); meshData.Vertices[i].TexC.x = theta / XM_2PI; meshData.Vertices[i].TexC.y = phi / XM_PI; // Partial derivative of P with respect to theta meshData.Vertices[i].TangentU.x = -radius*sinf(phi)*sinf(theta); meshData.Vertices[i].TangentU.y = 0.0f; meshData.Vertices[i].TangentU.z = +radius*sinf(phi)*cosf(theta); XMVECTOR T = XMLoadFloat3(&meshData.Vertices[i].TangentU); XMStoreFloat3(&meshData.Vertices[i].TangentU, XMVector3Normalize(T)); } }
/*------------------------------------------------------------------------------ create a quaterion that represents rotation about an axis (angle is radians) the resulting quaternion is normalized ------------------------------------------------------------------------------*/ Quaternion QuaternionRotationAxisAngle( Vector3 const &axis, float angle ) { Vector3 a = Normalize( axis) ; float halfAngle = angle * 0.5f; float s = sinf( halfAngle ); return Quaternion( s * a.x, s * a.y, s * a.z, cosf( halfAngle ) ); }
void GeometryGenerator::CreateSphere(float radius, UINT sliceCount, UINT stackCount, MeshData& meshData) { meshData.Vertices.clear(); meshData.Indices.clear(); // // Compute the vertices stating at the top pole and moving down the stacks. // // Poles: note that there will be texture coordinate distortion as there is // not a unique point on the texture map to assign to the pole when mapping // a rectangular texture onto a sphere. Vertex topVertex(0.0f, +radius, 0.0f, 0.0f, +1.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f); Vertex bottomVertex(0.0f, -radius, 0.0f, 0.0f, -1.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f); meshData.Vertices.push_back(topVertex); float phiStep = XM_PI / stackCount; float thetaStep = 2.0f*XM_PI / sliceCount; // Compute vertices for each stack ring (do not count the poles as rings). for (UINT i = 1; i <= stackCount - 1; ++i) { float phi = i*phiStep; // Vertices of ring. for (UINT j = 0; j <= sliceCount; ++j) { float theta = j*thetaStep; Vertex v; // spherical to cartesian v.Position.x = radius*sinf(phi)*cosf(theta); v.Position.y = radius*cosf(phi); v.Position.z = radius*sinf(phi)*sinf(theta); // Partial derivative of P with respect to theta v.TangentU.x = -radius*sinf(phi)*sinf(theta); v.TangentU.y = 0.0f; v.TangentU.z = +radius*sinf(phi)*cosf(theta); XMVECTOR T = XMLoadFloat3(&v.TangentU); XMStoreFloat3(&v.TangentU, XMVector3Normalize(T)); XMVECTOR p = XMLoadFloat3(&v.Position); XMStoreFloat3(&v.Normal, XMVector3Normalize(p)); v.TexC.x = theta / XM_2PI; v.TexC.y = phi / XM_PI; meshData.Vertices.push_back(v); } } meshData.Vertices.push_back(bottomVertex); // // Compute indices for top stack. The top stack was written first to the vertex buffer // and connects the top pole to the first ring. // for (UINT i = 1; i <= sliceCount; ++i) { meshData.Indices.push_back(0); meshData.Indices.push_back(i + 1); meshData.Indices.push_back(i); } // // Compute indices for inner stacks (not connected to poles). // // Offset the indices to the index of the first vertex in the first ring. // This is just skipping the top pole vertex. UINT baseIndex = 1; UINT ringVertexCount = sliceCount + 1; for (UINT i = 0; i < stackCount - 2; ++i) { for (UINT j = 0; j < sliceCount; ++j) { meshData.Indices.push_back(baseIndex + i*ringVertexCount + j); meshData.Indices.push_back(baseIndex + i*ringVertexCount + j + 1); meshData.Indices.push_back(baseIndex + (i + 1)*ringVertexCount + j); meshData.Indices.push_back(baseIndex + (i + 1)*ringVertexCount + j); meshData.Indices.push_back(baseIndex + i*ringVertexCount + j + 1); meshData.Indices.push_back(baseIndex + (i + 1)*ringVertexCount + j + 1); } } // // Compute indices for bottom stack. The bottom stack was written last to the vertex buffer // and connects the bottom pole to the bottom ring. // // South pole vertex was added last. UINT southPoleIndex = (UINT)meshData.Vertices.size() - 1; // Offset the indices to the index of the first vertex in the last ring. baseIndex = southPoleIndex - ringVertexCount; for (UINT i = 0; i < sliceCount; ++i) { meshData.Indices.push_back(southPoleIndex); meshData.Indices.push_back(baseIndex + i); meshData.Indices.push_back(baseIndex + i + 1); } }
void CCClipIn::startWithTarget(CCNode *pTarget) { CCAssert(dynamic_cast<CCClippingNode*>(pTarget) != NULL, "CCClipIn target must be a CCClippingNode instance"); CCActionInterval::startWithTarget(pTarget); // ensure the stencil of clipper is CCDrawNode CCClippingNode* clipper = (CCClippingNode*)pTarget; CCNode* stencil = clipper->getStencil(); if(dynamic_cast<CCDrawNode*>(stencil) == NULL) { clipper->setStencil(CCDrawNode::create()); } // direction radian float r = ccpToAngle(m_direction); m_cos = cosf(r); m_sin = sinf(r); // max distance along direction const CCSize& size = pTarget->getContentSize(); m_distance = size.width * fabsf(m_cos) + size.height * fabsf(m_sin); // calculate fixed edge endpoints if(r > M_PI_2) { float p1Len = size.width * cosf(r - M_PI_2); m_p1.x = size.width - p1Len * cosf(r - M_PI_2); m_p1.y = -p1Len * sinf(r - M_PI_2); float p2Len = size.height * sinf(r - M_PI_2); m_p2.x = size.width + p2Len * cosf(r - M_PI_2); m_p2.y = p2Len * sinf(r - M_PI_2); } else if(r >= 0) { float p1Len = size.height * m_cos; m_p1.x = p1Len * cosf(r + M_PI_2); m_p1.y = p1Len * sinf(r + M_PI_2); float p2Len = size.width * m_sin; m_p2.x = p2Len * cosf(r - M_PI_2); m_p2.y = p2Len * sinf(r - M_PI_2); } else if(r < -M_PI_2) { float p1Len = size.height * sinf(-r - M_PI_2); m_p1.x = size.width + p1Len * cosf(-r - M_PI_2); m_p1.y = size.height - p1Len * sinf(-r - M_PI_2); float p2Len = size.width * cosf(-r - M_PI_2); m_p2.x = size.width - p2Len * cosf(-r - M_PI_2); m_p2.y = size.height + p2Len * sinf(-r - M_PI_2); } else { float p1Len = size.width * sinf(-r); m_p1.x = p1Len * cosf(r + M_PI_2); m_p1.y = size.height + p1Len * sinf(r + M_PI_2); float p2Len = size.height * cosf(-r); m_p2.x = -p2Len * cosf(r + M_PI_2); m_p2.y = size.height - p2Len * sinf(r + M_PI_2); } }
/* Render::renderSceneShadowMap: shadow mapping */ void Render::renderSceneShadowMap(PMDObject *objs, const short *order, int num, Stage *stage, bool useMMDLikeCartoon, bool useCartoonRendering, float lightIntensity, const float *lightDirection, const float *lightColor, int shadowMappingTextureSize, bool shadowMappingLightFirst, float shadowMappingSelfDensity) { short i; GLint viewport[4]; /* store viewport */ GLdouble modelview[16]; /* store model view transform */ GLdouble projection[16]; /* store projection transform */ bool toonLight = true; #ifdef RENDER_SHADOWAUTOVIEW float eyeDist; btVector3 v; #endif /* RENDER_SHADOWAUTOVIEW */ static GLfloat lightdim[] = { 0.2f, 0.2f, 0.2f, 1.0f }; static const GLfloat lightblk[] = { 0.0f, 0.0f, 0.0f, 1.0f }; /* render the depth texture */ /* store the current viewport */ glGetIntegerv(GL_VIEWPORT, viewport); /* store the current projection matrix */ glGetDoublev(GL_PROJECTION_MATRIX, projection); /* switch to FBO for depth buffer rendering */ glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, m_fboID); /* clear the buffer */ /* clear only the depth buffer, since other buffers will not be used */ glClear(GL_DEPTH_BUFFER_BIT); /* set the viewport to the required texture size */ glViewport(0, 0, shadowMappingTextureSize, shadowMappingTextureSize); /* reset the projection matrix */ glMatrixMode(GL_PROJECTION); glLoadIdentity(); /* set the model view matrix to make the light position as eye point and capture the whole scene in the view */ glMatrixMode(GL_MODELVIEW); glLoadIdentity(); #ifdef RENDER_SHADOWAUTOVIEW /* set the distance to cover all the model range */ eyeDist = m_shadowMapAutoViewRadius / sinf(RENDER_SHADOWAUTOVIEWANGLE * 0.5f * 3.1415926f / 180.0f); /* set the perspective */ gluPerspective(RENDER_SHADOWAUTOVIEWANGLE, 1.0, 1.0, eyeDist + m_shadowMapAutoViewRadius + 50.0f); /* +50.0f is needed to cover the background */ /* the viewpoint should be at eyeDist far toward light direction from the model center */ v = m_lightVec * eyeDist + m_shadowMapAutoViewEyePoint; gluLookAt(v.x(), v.y(), v.z(), m_shadowMapAutoViewEyePoint.x(), m_shadowMapAutoViewEyePoint.y(), m_shadowMapAutoViewEyePoint.z(), 0.0, 1.0, 0.0); #else /* fixed view */ gluPerspective(25.0, 1.0, 1.0, 120.0); gluLookAt(30.0, 77.0, 30.0, 0.0, 17.0, 0.0, 0.0, 1.0, 0.0); #endif /* RENDER_SHADOWAUTOVIEW */ /* keep the current model view for later process */ glGetDoublev(GL_MODELVIEW_MATRIX, modelview); /* do not write into frame buffer other than depth information */ glColorMask(GL_FALSE, GL_FALSE, GL_FALSE, GL_FALSE); /* also, lighting is not needed */ glDisable(GL_LIGHTING); /* disable rendering the front surface to get the depth of back face */ glCullFace(GL_FRONT); /* disable alpha test */ glDisable(GL_ALPHA_TEST); /* we are now writing to depth texture using FBO, so disable the depth texture mapping here */ glActiveTextureARB(GL_TEXTURE3_ARB); glDisable(GL_TEXTURE_2D); glActiveTextureARB(GL_TEXTURE0_ARB); /* set polygon offset to avoid "moire" */ glEnable(GL_POLYGON_OFFSET_FILL); glPolygonOffset(4.0, 4.0); /* render objects for depth */ /* only objects that wants to drop shadow should be rendered here */ for (i = 0; i < num; i++) { if (objs[order[i]].isEnable() == true) { objs[order[i]].getPMDModel()->renderForShadow(); } } /* reset the polygon offset */ glDisable(GL_POLYGON_OFFSET_FILL); /* switch to default FBO */ glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, 0); /* revert configurations to normal rendering */ glViewport(viewport[0], viewport[1], viewport[2], viewport[3]); glMatrixMode(GL_PROJECTION); glLoadMatrixd(projection); glColorMask(GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE); glEnable(GL_LIGHTING); glCullFace(GL_BACK); glEnable(GL_ALPHA_TEST); /* clear all the buffers */ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT); /* render the full scene */ /* set model view matrix, as the same as normal rendering */ glMatrixMode(GL_MODELVIEW); glLoadIdentity(); glMultMatrixf(m_rotMatrix); /* render the whole scene */ if (shadowMappingLightFirst) { /* render light setting, later render only the shadow part with dark setting */ stage->renderBackground(); stage->renderFloor(); for (i = 0; i < num; i++) { if (objs[order[i]].isEnable() == true) { if (objs[order[i]].getPMDModel()->getToonFlag() == false && toonLight == true) { /* disable toon lighting */ updateLight(true, false, lightIntensity, lightDirection, lightColor); toonLight = false; } else if (objs[order[i]].getPMDModel()->getToonFlag() == true && toonLight == false) { /* enable toon lighting */ updateLight(useMMDLikeCartoon, useCartoonRendering, lightIntensity, lightDirection, lightColor); toonLight = true; } objs[order[i]].getPMDModel()->renderModel(); objs[order[i]].getPMDModel()->renderEdge(); } } if (toonLight == false) { /* restore toon lighting */ updateLight(useMMDLikeCartoon, useCartoonRendering, lightIntensity, lightDirection, lightColor); } } else { /* render in dark setting, later render only the non-shadow part with light setting */ /* light setting for non-toon objects */ lightdim[0] = lightdim[1] = lightdim[2] = 0.55f - 0.2f * shadowMappingSelfDensity; glLightfv(GL_LIGHT0, GL_DIFFUSE, lightdim); glLightfv(GL_LIGHT0, GL_AMBIENT, lightdim); glLightfv(GL_LIGHT0, GL_SPECULAR, lightblk); /* render the non-toon objects (back, floor, non-toon models) */ stage->renderBackground(); stage->renderFloor(); for (i = 0; i < num; i++) { if (objs[order[i]].isEnable() == true && objs[order[i]].getPMDModel()->getToonFlag() == false) objs[order[i]].getPMDModel()->renderModel(); } /* for toon objects, they should apply the model-defined toon texture color at texture coordinates (0, 0) for shadow rendering */ /* so restore the light setting */ if (useCartoonRendering == true) updateLight(useMMDLikeCartoon, useCartoonRendering, lightIntensity, lightDirection, lightColor); /* render the toon objects */ for (i = 0; i < num; i++) { if (objs[order[i]].isEnable() == true && objs[order[i]].getPMDModel()->getToonFlag() == true) { /* set texture coordinates for shadow mapping */ objs[order[i]].getPMDModel()->updateShadowColorTexCoord(shadowMappingSelfDensity); /* tell model to render with the shadow corrdinates */ objs[order[i]].getPMDModel()->setSelfShadowDrawing(true); /* render model and edge */ objs[order[i]].getPMDModel()->renderModel(); objs[order[i]].getPMDModel()->renderEdge(); /* disable shadow rendering */ objs[order[i]].getPMDModel()->setSelfShadowDrawing(false); } } if (useCartoonRendering == false) updateLight(useMMDLikeCartoon, useCartoonRendering, lightIntensity, lightDirection, lightColor); } /* render the part clipped by the depth texture */ /* activate the texture unit for shadow mapping and make it current */ glActiveTextureARB(GL_TEXTURE3_ARB); /* set texture matrix (note: matrices should be set in reverse order) */ glMatrixMode(GL_TEXTURE); glLoadIdentity(); /* move the range from [-1,1] to [0,1] */ glTranslated(0.5, 0.5, 0.5); glScaled(0.5, 0.5, 0.5); /* multiply the model view matrix when the depth texture was rendered */ glMultMatrixd(modelview); /* multiply the inverse matrix of current model view matrix */ glMultMatrixf(m_rotMatrixInv); /* revert to model view matrix mode */ glMatrixMode(GL_MODELVIEW); /* enable texture mapping with texture coordinate generation */ glEnable(GL_TEXTURE_2D); glEnable(GL_TEXTURE_GEN_S); glEnable(GL_TEXTURE_GEN_T); glEnable(GL_TEXTURE_GEN_R); glEnable(GL_TEXTURE_GEN_Q); /* bind the depth texture rendered at the first step */ glBindTexture(GL_TEXTURE_2D, m_depthTextureID); /* depth texture set up was done, now switch current texture unit to default */ glActiveTextureARB(GL_TEXTURE0_ARB); /* set depth func to allow overwrite for the same surface in the following rendering */ glDepthFunc(GL_LEQUAL); if (shadowMappingLightFirst) { /* the area clipped by depth texture by alpha test is dark part */ glAlphaFunc(GL_GEQUAL, 0.1f); /* light setting for non-toon objects */ lightdim[0] = lightdim[1] = lightdim[2] = 0.55f - 0.2f * shadowMappingSelfDensity; glLightfv(GL_LIGHT0, GL_DIFFUSE, lightdim); glLightfv(GL_LIGHT0, GL_AMBIENT, lightdim); glLightfv(GL_LIGHT0, GL_SPECULAR, lightblk); /* render the non-toon objects (back, floor, non-toon models) */ stage->renderBackground(); stage->renderFloor(); for (i = 0; i < num; i++) { if (objs[order[i]].isEnable() == true && objs[order[i]].getPMDModel()->getToonFlag() == false) objs[order[i]].getPMDModel()->renderModel(); } /* for toon objects, they should apply the model-defined toon texture color at texture coordinates (0, 0) for shadow rendering */ /* so restore the light setting */ if (useCartoonRendering == true) updateLight(useMMDLikeCartoon, useCartoonRendering, lightIntensity, lightDirection, lightColor); /* render the toon objects */ for (i = 0; i < num; i++) { if (objs[order[i]].isEnable() == true && objs[order[i]].getPMDModel()->getToonFlag() == true) { /* set texture coordinates for shadow mapping */ objs[order[i]].getPMDModel()->updateShadowColorTexCoord(shadowMappingSelfDensity); /* tell model to render with the shadow corrdinates */ objs[order[i]].getPMDModel()->setSelfShadowDrawing(true); /* render model and edge */ objs[order[i]].getPMDModel()->renderModel(); /* disable shadow rendering */ objs[order[i]].getPMDModel()->setSelfShadowDrawing(false); } } if (useCartoonRendering == false) updateLight(useMMDLikeCartoon, useCartoonRendering, lightIntensity, lightDirection, lightColor); } else { /* the area clipped by depth texture by alpha test is light part */ glAlphaFunc(GL_GEQUAL, 0.001f); stage->renderBackground(); stage->renderFloor(); for (i = 0; i < num; i++) { if (objs[order[i]].isEnable() == true) { if (objs[order[i]].getPMDModel()->getToonFlag() == false && toonLight == true) { /* disable toon lighting */ updateLight(true, false, lightIntensity, lightDirection, lightColor); toonLight = false; } else if (objs[order[i]].getPMDModel()->getToonFlag() == true && toonLight == false) { /* enable toon lighting */ updateLight(useMMDLikeCartoon, useCartoonRendering, lightIntensity, lightDirection, lightColor); toonLight = true; } objs[order[i]].getPMDModel()->renderModel(); } } if (toonLight == false) { /* restore toon lighting */ updateLight(useMMDLikeCartoon, useCartoonRendering, lightIntensity, lightDirection, lightColor); } } /* reset settings */ glDepthFunc(GL_LESS); glAlphaFunc(GL_GEQUAL, 0.05f); glActiveTextureARB(GL_TEXTURE3_ARB); glDisable(GL_TEXTURE_GEN_S); glDisable(GL_TEXTURE_GEN_T); glDisable(GL_TEXTURE_GEN_R); glDisable(GL_TEXTURE_GEN_Q); glDisable(GL_TEXTURE_2D); glActiveTextureARB(GL_TEXTURE0_ARB); }
void display(void) { static int time = 0; int i; glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); glLoadIdentity(); #define RAD(x) (((x)*M_PI)/180.) gluLookAt(-sinf(RAD(rotx)) * 5.5, transy, cosf(RAD(rotx)) * 5.5, 0., 0., 0., 0., 1., 0.); /* * floor */ glColor4f(0.f, .2f, 0.f, 1.f); glBegin(GL_POLYGON); glVertex3f(-4.0, -1.0, -4.0); glVertex3f(4.0, -1.0, -4.0); glVertex3f(4.0, -1.0, 4.0); glVertex3f(-4.0, -1.0, 4.0); glEnd(); glEnable(GL_LIGHTING); glPushMatrix(); glColor3f(.3f, .3f, .3f); glPushMatrix(); glTranslatef(-1.f, -1.f + .2f, -1.5f); glScalef(.2f, .2f, .2f); /* * the box */ for (i = 0; i < 10; i++) { float ax = (corners[3 * i][0] + corners[3 * i + 1][0] + corners[3 * i + 2][0]) / 3, ay = (corners[3 * i][1] + corners[3 * i + 1][1] + corners[3 * i + 2][1]) / 3, az = (corners[3 * i][2] + corners[3 * i + 1][2] + corners[3 * i + 2][2]) / 3; glPushMatrix(); glTranslatef(ax, ay, az); glRotatef(omega[i] * delta, axes[i][0], axes[i][1], axes[i][2]); glTranslatef(-ax, -ay, -az); glBegin(GL_TRIANGLES); glNormal3fv(normals[i]); glVertex3fv(corners[3 * i]); glVertex3fv(corners[3 * i + 1]); glVertex3fv(corners[3 * i + 2]); glEnd(); glPopMatrix(); } /* * base of the box (stays there) */ glBegin(GL_QUADS); glNormal3f(0.0, 1.0, 0.0); glVertex3f(-1.0, -1.0, -1.0); glVertex3f(-1.0, -1.0, 1.0); glVertex3f(1.0, -1.0, 1.0); glVertex3f(1.0, -1.0, -1.0); glEnd(); glPopMatrix(); glDisable(GL_LIGHTING); glTranslatef(-1.f, -1.f, -1.5f); glEnable(GL_TEXTURE_2D); glBindTexture(GL_TEXTURE_2D, texnames[1]); glDepthMask(0); #if 0 for (i = smokes - 1; i >= 0; i--) { int t = smoke[i]; glPushMatrix(); glTranslatef(0.05 * sin(M_PI * (t + time) / 64.0), 0.02 * t, 0.05 * cos(M_PI * (t + time) / 64.0)); glScalef((t + 60) / MAX_TIME, (t + 60) / MAX_TIME, 1.0); glColor4f(0.2, 0.2, 0.2, 10.0 / t); glBegin(GL_QUADS); glTexCoord2f(0, 0); glVertex3f(-1.0, 0.0, -1.0); glTexCoord2f(1, 0); glVertex3f(1.0, 0.0, -1.0); glTexCoord2f(1, 1); glVertex3f(1.0, 0.0, 1.0); glTexCoord2f(0, 1); glVertex3f(-1.0, 0.0, 1.0); glEnd(); glPopMatrix(); smoke[i]++; } #endif time++; glDisable(GL_TEXTURE_2D); glDisable(GL_DEPTH_TEST); exploder(0.f, 0.f, 0.f, size, intensity, opacity, 0.f, 3.f); exploder(0.f, 0.f, 1.f, size, intensity, opacity, .2f, 1.4f); exploder(0.f, .8f, 1.4f, size, intensity, opacity, .4f, 1.6f); exploder(0.f, 1.2f, 0.f, size - .4f, intensity, opacity, .4f, 2.f); exploder(1.6f, .3f, 0.f, size - 1.f, intensity, opacity, .5f, 3.f); glEnable(GL_DEPTH_TEST); glDepthMask(1); glPopMatrix(); glDisable(GL_TEXTURE_2D); glutSwapBuffers(); }
btScalar duWater::getWaterLevel(btScalar pos_x, btScalar pos_z, int wrapperNum) { // get water wrapper by index duWater::WaterWrapper *wWrapper = wrapperByInd(wrapperNum); if (!wWrapper) return btScalar(0.0); if (!wWrapper->wavesHeight) return wWrapper->waterLevel; float time = m_time; ////////// DISTANT WAVES ////////// // first component float noise_coords[2]; duWaterDynInfo* dynInfo = wWrapper->dynamicsInfo; noise_coords[0] = dynInfo->dst_noise_scale0 * (pos_x + dynInfo->dst_noise_freq0 * time); noise_coords[1] = dynInfo->dst_noise_scale0 * (pos_z + dynInfo->dst_noise_freq0 * time); float noise1 = snoise(noise_coords); // second component noise_coords[0] = dynInfo->dst_noise_scale1 * (pos_z - dynInfo->dst_noise_freq1 * time); noise_coords[1] = dynInfo->dst_noise_scale1 * (pos_x - dynInfo->dst_noise_freq1 * time); float noise2 = snoise(noise_coords); float dist_waves = wWrapper->wavesHeight * noise1 * noise2; float wave_height; if (wWrapper->wDistArray) { ////////// SHORE WAVES ////////// // get coordinates in texture pixels double x = (pos_x - wWrapper->shoreMapCenterX) / wWrapper->shoreMapSizeX; double z = (wWrapper->shoreMapCenterZ + pos_z) / wWrapper->shoreMapSizeZ; x += 0.5f; z += 0.5f; // if position is out of boundings, consider that shore dist = 1 if (x > 1.f || x < 0.f || z > 1.f || z < 0.f) wave_height = dist_waves; else { // get coordinates in pixels int array_width = wWrapper->shoreMapTexSize; x *= array_width - .5f; z *= array_width - .5f; double floor_px; double floor_py; float fract_px = modf(x, &floor_px); float fract_py = modf(z, &floor_py); int px = static_cast<int>(floor_px); int py = static_cast<int>(floor_py); btScalar *distArray = wWrapper->wDistArray; int up_lim = array_width - 1; float dist00 = distArray[py * array_width + px]; float dist10 = distArray[py * array_width + btMin(px + 1, up_lim)]; float dist01 = distArray[btMin(py + 1, up_lim) * array_width + px]; float dist11 = distArray[btMin(py + 1, up_lim) * array_width + btMin(px + 1, up_lim)]; // distance on bottom, top edge float dist0010 = dist00 * (1.f - fract_px) + dist10 * fract_px; float dist0111 = dist01 * (1.f - fract_px) + dist11 * fract_px; float shore_dist = dist0010 * (1.f - fract_py) + dist0111 * fract_py; float shore_waves_length = wWrapper->wavesLength / float(wWrapper->maxShoreDist) / M_PI; float waves_coords[2] = {dynInfo->dir_noise_scale * (pos_x + dynInfo->dir_noise_freq * time), dynInfo->dir_noise_scale * (pos_z + dynInfo->dir_noise_freq * time)}; float dist_fact = sqrt(shore_dist); float shore_dir_waves = wWrapper->wavesHeight * fmax(shore_dist, dynInfo->dir_min_shore_fac) * sinf((dist_fact / shore_waves_length + dynInfo->dir_freq * time)) * fmax(snoise(waves_coords), dynInfo->dir_min_noise_fac); // mix two types of waves basing on distance to the shore float mix_rate = btMax(dist_fact, dynInfo->dst_min_fac); wave_height = shore_dir_waves * (1 - mix_rate) + dist_waves * mix_rate; } } else wave_height = dist_waves; btScalar cur_water_level = wWrapper->waterLevel + wave_height; return cur_water_level; }
float A(float x, float phi, float h){ return asinhf((x/h)*sinf(phi)); }
void get_sight_vector(float rx, float ry, float *vx, float *vy, float *vz) { float m = cosf(ry); *vx = cosf(rx - RADIANS(90)) * m; *vy = sinf(ry); *vz = sinf(rx - RADIANS(90)) * m; }
// analytical marginal inverse CDF for inverse squared sampling points vrl, phi is angle between camera vector and vrl vector float invSqrCDF(float r,float phi,float h, float v0, float v1){ return h*sinhf(Lerp(r,A(v0, phi, h), A(v1,phi,h)))/sinf(phi); }
void Light::OnUpdate(sf::Time interval) { std::lock_guard<std::recursive_mutex> lg(m_mutex); if (!m_active || !m_render || !GetParent()->IsRender()) return; sf::Vector2f pos = GetGlobalPosition(); const auto& view = m_scene->GetGame()->GetWindow()->getView(); auto vr = sf::FloatRect(view.getCenter().x - view.getSize().x / 2, view.getCenter().y - view.getSize().y / 2, view.getSize().x, view.getSize().y); if (!vr.intersects(sf::Rect<float>(pos.x - m_radius, pos.y - m_radius, 2 * m_radius, 2 * m_radius))) { m_blocked = true; return; } b2AABB center; center.lowerBound.x = pos.x / m_scene->GetPixelMeterRatio(); center.lowerBound.y = pos.y / m_scene->GetPixelMeterRatio(); float onep = (1.f / m_scene->GetPixelMeterRatio()); center.upperBound.x = center.lowerBound.x + onep; center.upperBound.y = center.lowerBound.y + onep; CenterQuery cq(center.lowerBound.x, center.lowerBound.y); GetScene()->GetWorld()->QueryAABB(&cq, center); if (cq.hit) { cq.node->OnLightRay.Fire(this); m_blocked = true; return; } m_blocked = false; float rayDistance = (2 - cosf(m_openingAngle / (m_rayCount - 1))) * m_radius / m_scene->GetPixelMeterRatio(); b2Vec2 basePos(pos.x / m_scene->GetPixelMeterRatio(), pos.y / m_scene->GetPixelMeterRatio()); struct edgeData { b2Vec2 pos; b2Fixture* fixture; int vertex; }; std::map<int, edgeData> edges; auto EdgeFromPos = [&, this](const b2Vec2& p) -> edgeData* { float angle = atan2f(basePos.y - p.y, basePos.x - p.x) + fPI; if (angle > m_angle + m_openingAngle || angle < m_angle) { return nullptr; } int i = static_cast<int>(angle / fPI * 180) % 360; auto it = edges.find(i); if (it == edges.end()) { float r = m_radius / m_scene->GetPixelMeterRatio(); edgeData def{basePos + b2Vec2(r, r), nullptr, 0}; edges.insert(std::make_pair(i, def)); return &edges.find(i)->second; } return &it->second; }; auto aabbCallback = engine::MakeAABBQueryCallback([&, this](b2Fixture* fixture) -> bool { if (!fixture->GetBody()->GetUserData() || static_cast<Node*>(fixture->GetBody()->GetUserData())->IsOpaque()) return true; if (fixture->GetShape()->GetType() == b2Shape::e_polygon) { b2PolygonShape* shape = static_cast<b2PolygonShape*>(fixture->GetShape()); for (size_t i = 0; i < shape->GetVertexCount(); i++) { const b2Vec2& vertex = shape->GetVertex(i); b2Vec2 vertexPos = b2Mul(fixture->GetBody()->GetTransform(), vertex); auto vLen = (vertexPos - basePos).Length(); if (vLen < m_radius / m_scene->GetPixelMeterRatio()) { auto edge = EdgeFromPos(vertexPos); if (!edge) continue; // Is the vertex we found better b2Vec2 dif = edge->pos - basePos; if (vLen < dif.Length()) { edge->pos = vertexPos; } edge->fixture = fixture; edge->vertex = i; } } } return true; }); b2AABB aabb; aabb.lowerBound = b2Vec2((pos.x - m_radius) / m_scene->GetPixelMeterRatio(), (pos.y - m_radius) / m_scene->GetPixelMeterRatio()); aabb.upperBound = b2Vec2((pos.x + m_radius) / m_scene->GetPixelMeterRatio(), (pos.y + m_radius) / m_scene->GetPixelMeterRatio()); m_scene->GetWorld()->QueryAABB(&aabbCallback, aabb); float step = m_openingAngle / static_cast<float> (m_rayCount - 1); float angle = m_angle; auto it = edges.begin(); m_vertices.resize(1); // keep the center vertex float f = 1.0; Node* hitNode = nullptr; auto rayCastCallback = MakeRayCastCallback([&](b2Fixture* fixture, const b2Vec2& point, const b2Vec2& normal, float32 fraction) { Node* n = static_cast<Node*> (fixture->GetBody()->GetUserData()); if (n && !n->IsOpaque() && fraction < f) { f = fraction; hitNode = n; } return f; }); sf::Vertex v; float edgeAngle = 0; v.position.x = cosf(angle) * m_radius; v.position.y = sinf(angle) * m_radius; f = 1.0; m_scene->GetWorld()->RayCast(&rayCastCallback, basePos, basePos + b2Vec2(v.position.x / m_scene->GetPixelMeterRatio(), v.position.y / m_scene->GetPixelMeterRatio())); if (hitNode) hitNode->OnLightRay.Fire(this); v.position.x *= f; v.position.y *= f; AssignLightColor(v, f, m_lightColor); m_vertices.push_back(v); angle += step; for (size_t i = 2; i < m_rayCount + 1; i++, angle += step) { bool had = false; while (it != edges.end()) { edgeAngle = b2Angle(basePos, it->second.pos); if (edgeAngle < 0) edgeAngle += fPI * 2; if (angle < edgeAngle || edgeAngle >= angle + step) { break; } if (it->second.fixture == nullptr) { ++it; continue; } f = 1.0; m_scene->GetWorld()->RayCast(&rayCastCallback, basePos, it->second.pos); // Check if this edge is blocked, skip if it is if (f < 1.0 - 10.0f / m_radius) { ++it; continue; } if (hitNode) hitNode->OnLightRay.Fire(this); had = true; float edgeLengthPct = m_scene->MeterToPixel((basePos - it->second.pos).Length()) / m_radius + (10.0f / m_radius); auto addPoint = [&, this](b2Vec2 point) { v.position.x = m_scene->MeterToPixel(point.x) - pos.x; v.position.y = m_scene->MeterToPixel(point.y) - pos.y; f = sqrtf(v.position.x * v.position.x + v.position.y * v.position.y) / m_radius; AssignLightColor(v, f, m_lightColor); m_vertices.push_back(v); }; // Check surrounding edges by using half a degree differences float checkAngle = edgeAngle - (fPI / 840.f); if (checkAngle > 0) { f = 1.0; b2Vec2 edge = b2Vec2(cosf(edgeAngle - (fPI / 840.f)), sinf(edgeAngle - (fPI / 840.f))); edge *= (m_radius / m_scene->GetPixelMeterRatio()); edge += basePos; m_scene->GetWorld()->RayCast(&rayCastCallback, basePos, edge); if (f > edgeLengthPct) { b2Vec2 p = (basePos - edge); p *= -f; addPoint(basePos + p); } } addPoint(it->second.pos); f = 1.0; checkAngle = edgeAngle + (fPI / 840.f); if (checkAngle < fPI * 2) { b2Vec2 edge = b2Vec2(cosf(checkAngle), sinf(checkAngle)); edge *= (m_radius / m_scene->GetPixelMeterRatio()); edge += basePos; m_scene->GetWorld()->RayCast(&rayCastCallback, basePos, edge); if (f > edgeLengthPct) { b2Vec2 p = (basePos - edge); p *= -f; addPoint(basePos + p); } } ++it; } // Prevent overlapping rays if (!had || had && !floatEqual(edgeAngle, angle, fPI / 180.0f) || i == 1 || i == m_rayCount) { v.position.x = cosf(angle) * m_radius; v.position.y = sinf(angle) * m_radius; f = 1.0; m_scene->GetWorld()->RayCast(&rayCastCallback, basePos, basePos + b2Vec2(v.position.x / m_scene->GetPixelMeterRatio(), v.position.y / m_scene->GetPixelMeterRatio())); if (hitNode) hitNode->OnLightRay.Fire(this); v.position.x *= f; v.position.y *= f; AssignLightColor(v, f, m_lightColor); m_vertices.push_back(v); } } }
void Standard::update_mc_state() { VtolType::update_mc_state(); // enable MC motors here in case we transitioned directly to MC mode if (_flag_enable_mc_motors) { set_max_mc(2000); set_idle_mc(); _flag_enable_mc_motors = false; } // if the thrust scale param is zero then the pusher-for-pitch strategy is disabled and we can return if (_params_standard.forward_thrust_scale < FLT_EPSILON) { return; } matrix::Dcmf R(matrix::Quatf(_v_att->q)); matrix::Dcmf R_sp(&_v_att_sp->R_body[0]); matrix::Eulerf euler(R); matrix::Eulerf euler_sp(R_sp); _pusher_throttle = 0.0f; // direction of desired body z axis represented in earth frame matrix::Vector3f body_z_sp(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); // rotate desired body z axis into new frame which is rotated in z by the current // heading of the vehicle. we refer to this as the heading frame. matrix::Dcmf R_yaw = matrix::Eulerf(0.0f, 0.0f, -euler(2)); body_z_sp = R_yaw * body_z_sp; body_z_sp.normalize(); // calculate the desired pitch seen in the heading frame // this value corresponds to the amount the vehicle would try to pitch forward float pitch_forward = asinf(body_z_sp(0)); // only allow pitching forward up to threshold, the rest of the desired // forward acceleration will be compensated by the pusher if (pitch_forward < -_params_standard.down_pitch_max) { // desired roll angle in heading frame stays the same float roll_new = -atan2f(body_z_sp(1), body_z_sp(2)); _pusher_throttle = (sinf(-pitch_forward) - sinf(_params_standard.down_pitch_max)) * _v_att_sp->thrust * _params_standard.forward_thrust_scale; // limit desired pitch float pitch_new = -_params_standard.down_pitch_max; // create corrected desired body z axis in heading frame matrix::Dcmf R_tmp = matrix::Eulerf(roll_new, pitch_new, 0.0f); matrix::Vector3f tilt_new(R_tmp(0, 2), R_tmp(1, 2), R_tmp(2, 2)); // rotate the vector into a new frame which is rotated in z by the desired heading // with respect to the earh frame. float yaw_error = _wrap_pi(euler_sp(2) - euler(2)); matrix::Dcmf R_yaw_correction = matrix::Eulerf(0.0f, 0.0f, -yaw_error); tilt_new = R_yaw_correction * tilt_new; // now extract roll and pitch setpoints float pitch = asinf(tilt_new(0)); float roll = -atan2f(tilt_new(1), tilt_new(2)); R_sp = matrix::Eulerf(roll, pitch, euler_sp(2)); matrix::Quatf q_sp(R_sp); memcpy(&_v_att_sp->R_body[0], &R_sp._data[0], sizeof(_v_att_sp->R_body)); memcpy(&_v_att_sp->q_d[0], &q_sp._data[0], sizeof(_v_att_sp->q_d)); } _pusher_throttle = _pusher_throttle < 0.0f ? 0.0f : _pusher_throttle; }