kmVec3* kmPlaneGetIntersection(kmVec3* pOut, const kmPlane* p1, const kmPlane* p2, const kmPlane* p3) { kmVec3 n1, n2, n3, cross; kmVec3 r1, r2, r3; double denom = 0; kmVec3Fill(&n1, p1->a, p1->b, p1->c); kmVec3Fill(&n2, p2->a, p2->b, p2->c); kmVec3Fill(&n3, p3->a, p3->b, p3->c); kmVec3Cross(&cross, &n2, &n3); denom = kmVec3Dot(&n1, &cross); if (kmAlmostEqual(denom, 0.0)) { return NULL; } kmVec3Cross(&r1, &n2, &n3); kmVec3Cross(&r2, &n3, &n1); kmVec3Cross(&r3, &n1, &n2); kmVec3Scale(&r1, &r1, -p1->d); kmVec3Scale(&r2, &r2, p2->d); kmVec3Scale(&r3, &r3, p3->d); kmVec3Subtract(pOut, &r1, &r2); kmVec3Subtract(pOut, pOut, &r3); kmVec3Scale(pOut, pOut, 1.0 / denom); /*p = -d1 * ( n2.Cross ( n3 ) ) – d2 * ( n3.Cross ( n1 ) ) – d3 * ( n1.Cross ( n2 ) ) / denom;*/ return pOut; }
kmMat3* kmMat3LookAt(kmMat3* pOut, const kmVec3* pEye, const kmVec3* pCenter, const kmVec3* pUp) { kmVec3 f, up, s, u; kmVec3Subtract(&f, pCenter, pEye); kmVec3Normalize(&f, &f); kmVec3Assign(&up, pUp); kmVec3Normalize(&up, &up); kmVec3Cross(&s, &f, &up); kmVec3Normalize(&s, &s); kmVec3Cross(&u, &s, &f); kmVec3Normalize(&s, &s); pOut->mat[0] = s.x; pOut->mat[3] = s.y; pOut->mat[6] = s.z; pOut->mat[1] = u.x; pOut->mat[4] = u.y; pOut->mat[7] = u.z; pOut->mat[2] = -f.x; pOut->mat[5] = -f.y; pOut->mat[8] = -f.z; return pOut; }
void ViewTransform::LazyAdjust() const { if(!_dirty) return; kmVec3Subtract(&_adjustDir, &_focus, &_position); kmVec3Normalize(&_adjustDir, &_adjustDir); kmVec3Cross(&_adjustRight, &_adjustDir, &_up); kmVec3Normalize(&_adjustRight, &_adjustRight); kmVec3Cross(&_adjustUp, &_adjustRight, &_adjustDir); kmVec3Normalize(&_adjustUp, &_adjustUp); _dirty = false; }
kmBool kmRay3IntersectTriangle(const kmRay3* ray, const kmVec3* v0, const kmVec3* v1, const kmVec3* v2, kmVec3* intersection, kmVec3* normal, kmScalar* distance) { kmVec3 e1, e2, pvec, tvec, qvec, dir; kmScalar det, inv_det, u, v, t; kmVec3Normalize(&dir, &ray->dir); kmVec3Subtract(&e1, v1, v0); kmVec3Subtract(&e2, v2, v0); kmVec3Cross(&pvec, &dir, &e2); det = kmVec3Dot(&e1, &pvec); /* Backfacing, discard. */ if(det < kmEpsilon) { return KM_FALSE; } if(kmAlmostEqual(det, 0)) { return KM_FALSE; } inv_det = 1.0 / det; kmVec3Subtract(&tvec, &ray->start, v0); u = inv_det * kmVec3Dot(&tvec, &pvec); if(u < 0.0 || u > 1.0) { return KM_FALSE; } kmVec3Cross(&qvec, &tvec, &e1); v = inv_det * kmVec3Dot(&dir, &qvec); if(v < 0.0 || (u + v) > 1.0) { return KM_FALSE; } t = inv_det * kmVec3Dot(&e2, &qvec); if(t > kmEpsilon && (t*t) <= kmVec3LengthSq(&ray->dir)) { kmVec3 scaled; *distance = t; /* Distance */ kmVec3Cross(normal, &e1, &e2); /* Surface normal of collision */ kmVec3Normalize(normal, normal); kmVec3Normalize(&scaled, &dir); kmVec3Scale(&scaled, &scaled, *distance); kmVec3Add(intersection, &ray->start, &scaled); return KM_TRUE; } return KM_FALSE; }
/** * Creates a plane from 3 points. The result is stored in pOut. * pOut is returned. */ kmPlane* const kmPlaneFromPoints(kmPlane* pOut, const kmVec3* p1, const kmVec3* p2, const kmVec3* p3) { /* v = (B − A) × (C − A) n = 1⁄|v| v Outa = nx Outb = ny Outc = nz Outd = −n⋅A */ kmVec3 n, v1, v2; kmVec3Subtract(&v1, p2, p1); //Create the vectors for the 2 sides of the triangle kmVec3Subtract(&v2, p3, p1); kmVec3Cross(&n, &v1, &v2); //Use the cross product to get the normal kmVec3Normalize(&n, &n); //Normalize it and assign to pOut->m_N pOut->a = n.x; pOut->b = n.y; pOut->c = n.z; pOut->d = kmVec3Dot(kmVec3Scale(&n, &n, -1.0), p1); return pOut; }
kmQuaternion *sls_trackball_calc_quat(kmQuaternion *out, float trackball_radius, float trackball_speed, kmVec2 const *p1, kmVec2 const *p2) { //sls_log_info("p1 %f %f, p2 %f %f", p1->x, p1->y, p2->x, p2->y); kmVec3 axis; kmVec3 _p1, _p2, dir; float phi; float t; float epsilon = 0.001; if (sls_vec2_near(p1, p2, epsilon)) { // zero rotation kmQuaternionIdentity(out); return out; } _p1 = (kmVec3){p1->x, p1->y, sls_tb_project_to_sphere(trackball_radius, p1)}; _p2 = (kmVec3){p2->x, p2->y, sls_tb_project_to_sphere(trackball_radius, p2)}; kmVec3Subtract(&dir, &_p1, &_p2); kmVec3Cross(&axis, &_p2, &_p1); t = kmVec3Length(&dir) / (2.0f * trackball_radius); t = fminf(fmaxf(-1.f, t), 1.f); phi = 2.0f * asinf(t) * trackball_speed; kmQuaternion tmp_a, tmp_b; kmQuaternionRotationAxisAngle(&tmp_a, &axis, phi); tmp_b = *out; return kmQuaternionMultiply(out, &tmp_a, &tmp_b); ; }
kmVec3* kmQuaternionMultiplyVec3(kmVec3* pOut, const kmQuaternion* q, const kmVec3* v) { kmVec3 uv, uuv, qvec; qvec.x = q->x; qvec.y = q->y; qvec.z = q->z; kmVec3Cross(&uv, &qvec, v); kmVec3Cross(&uuv, &qvec, &uv); kmVec3Scale(&uv, &uv, (2.0f * q->w)); kmVec3Scale(&uuv, &uuv, 2.0f); kmVec3Add(pOut, v, &uv); kmVec3Add(pOut, pOut, &uuv); return pOut; }
/** * Builds a translation matrix in the same way as gluLookAt() * the resulting matrix is stored in pOut. pOut is returned. */ kmMat4* const kmMat4LookAt(kmMat4* pOut, const kmVec3* pEye, const kmVec3* pCenter, const kmVec3* pUp) { kmVec3 f, up, s, u; kmMat4 translate; kmVec3Subtract(&f, pCenter, pEye); kmVec3Normalize(&f, &f); kmVec3Assign(&up, pUp); kmVec3Normalize(&up, &up); kmVec3Cross(&s, &f, &up); kmVec3Normalize(&s, &s); kmVec3Cross(&u, &s, &f); kmVec3Normalize(&s, &s); kmMat4Identity(pOut); pOut->mat[0] = s.x; pOut->mat[4] = s.y; pOut->mat[8] = s.z; pOut->mat[1] = u.x; pOut->mat[5] = u.y; pOut->mat[9] = u.z; pOut->mat[2] = -f.x; pOut->mat[6] = -f.y; pOut->mat[10] = -f.z; kmMat4Translation(&translate, -pEye->x, -pEye->y, -pEye->z); kmMat4Multiply(pOut, pOut, &translate); return pOut; }
/** * Builds a translation matrix in the same way as gluLookAt() * the resulting matrix is stored in pOut. pOut is returned. */ kmMat4* kmMat4LookAt(kmMat4* pOut, const kmVec3* pEye, const kmVec3* pCenter, const kmVec3* pUp) { kmVec3 f; kmVec3 s; kmVec3 u; kmVec3Subtract(&f, pCenter, pEye); kmVec3Normalize(&f, &f); kmVec3Cross(&s, &f, pUp); kmVec3Normalize(&s, &s); kmVec3Cross(&u, &s, &f); pOut->mat[0] = s.x; pOut->mat[1] = u.x; pOut->mat[2] = -f.x; pOut->mat[3] = 0.0; pOut->mat[4] = s.y; pOut->mat[5] = u.y; pOut->mat[6] = -f.y; pOut->mat[7] = 0.0; pOut->mat[8] = s.z; pOut->mat[9] = u.z; pOut->mat[10] = -f.z; pOut->mat[11] = 0.0; pOut->mat[12] = -kmVec3Dot(&s, pEye); pOut->mat[13] = -kmVec3Dot(&u, pEye); pOut->mat[14] = kmVec3Dot(&f, pEye); pOut->mat[15] = 1.0; return pOut; }
/* * Returns a Quaternion representing the angle between two vectors */ kmQuaternion* kmQuaternionBetweenVec3(kmQuaternion* pOut, const kmVec3* u, const kmVec3* v) { kmVec3 w; kmScalar len; kmQuaternion q; if(kmVec3AreEqual(u, v)) { kmQuaternionIdentity(pOut); return pOut; } len = sqrtf(kmVec3LengthSq(u) * kmVec3LengthSq(v)); kmVec3Cross(&w, u, v); kmQuaternionFill(&q, w.x, w.y, w.z, kmVec3Dot(u, v) + len); return kmQuaternionNormalize(pOut, &q); }
void Frustum::setupProjectionPerspective(const ViewTransform& view, float left, float right, float top, float bottom, float nearPlane, float farPlane) { kmVec3 cc = view.getPosition(); kmVec3 cDir = view.getDirection(); kmVec3 cRight = view.getRight(); kmVec3 cUp = view.getUp(); kmVec3Normalize(&cDir, &cDir); kmVec3Normalize(&cRight, &cRight); kmVec3Normalize(&cUp, &cUp); kmVec3 nearCenter; kmVec3 farCenter; kmVec3Scale(&nearCenter, &cDir, nearPlane); kmVec3Add(&nearCenter, &nearCenter, &cc); kmVec3Scale(&farCenter, &cDir, farPlane); kmVec3Add(&farCenter, &farCenter, &cc); //near { kmPlaneFromPointAndNormal(&_frustumPlanes[FrustumPlane::FRUSTUM_NEAR], &nearCenter, &cDir); } //far { kmVec3 normal; kmVec3Scale(&normal, &cDir, -1); kmPlaneFromPointAndNormal(&_frustumPlanes[FrustumPlane::FRUSTUM_FAR], &farCenter, &normal); } //left { kmVec3 point; kmVec3Scale(&point, &cRight, left); kmVec3Add(&point, &point, &nearCenter); kmVec3 normal; kmVec3Subtract(&normal, &point, &cc); kmVec3Cross(&normal, &normal, &cUp); kmVec3Normalize(&normal, &normal); kmPlaneFromPointAndNormal(&_frustumPlanes[FrustumPlane::FRUSTUM_LEFT], &point, &normal); } //right { kmVec3 point; kmVec3Scale(&point, &cRight, right); kmVec3Add(&point, &point, &nearCenter); kmVec3 normal; kmVec3Subtract(&normal, &point, &cc); kmVec3Cross(&normal, &cUp, &normal); kmVec3Normalize(&normal, &normal); kmPlaneFromPointAndNormal(&_frustumPlanes[FrustumPlane::FRUSTUM_RIGHT], &point, &normal); } //bottom { kmVec3 point; kmVec3Scale(&point, &cUp, bottom); kmVec3Add(&point, &point, &nearCenter); kmVec3 normal; kmVec3Subtract(&normal, &point, &cc); kmVec3Cross(&normal, &cRight, &normal); kmVec3Normalize(&normal, &normal); kmPlaneFromPointAndNormal(&_frustumPlanes[FrustumPlane::FRUSTUM_BOTTOM], &point, &normal); } //top { kmVec3 point; kmVec3Scale(&point, &cUp, top); kmVec3Add(&point, &point, &nearCenter); kmVec3 normal; kmVec3Subtract(&normal, &point, &cc); kmVec3Cross(&normal, &normal, &cRight); kmVec3Normalize(&normal, &normal); kmPlaneFromPointAndNormal(&_frustumPlanes[FrustumPlane::FRUSTUM_TOP], &point, &normal); } }
kmQuaternion* kmQuaternionRotationBetweenVec3(kmQuaternion* pOut, const kmVec3* vec1, const kmVec3* vec2, const kmVec3* fallback) { kmVec3 v1, v2; kmScalar a; kmVec3Assign(&v1, vec1); kmVec3Assign(&v2, vec2); kmVec3Normalize(&v1, &v1); kmVec3Normalize(&v2, &v2); a = kmVec3Dot(&v1, &v2); if (a >= 1.0) { kmQuaternionIdentity(pOut); return pOut; } if (a < (1e-6f - 1.0f)) { if (fabs(kmVec3LengthSq(fallback)) < kmEpsilon) { kmQuaternionRotationAxisAngle(pOut, fallback, kmPI); } else { kmVec3 axis; kmVec3 X; X.x = 1.0; X.y = 0.0; X.z = 0.0; kmVec3Cross(&axis, &X, vec1); /*If axis is zero*/ if (fabs(kmVec3LengthSq(&axis)) < kmEpsilon) { kmVec3 Y; Y.x = 0.0; Y.y = 1.0; Y.z = 0.0; kmVec3Cross(&axis, &Y, vec1); } kmVec3Normalize(&axis, &axis); kmQuaternionRotationAxisAngle(pOut, &axis, kmPI); } } else { kmScalar s = sqrtf((1+a) * 2); kmScalar invs = 1 / s; kmVec3 c; kmVec3Cross(&c, &v1, &v2); pOut->x = c.x * invs; pOut->y = c.y * invs; pOut->z = c.z * invs; pOut->w = s * 0.5f; kmQuaternionNormalize(pOut, pOut); } return pOut; }
int process_inputs() { // Get mouse position double xpos, ypos; glfwGetCursorPos(mainWindow, &xpos, &ypos); // Reset mouse position for next frame glfwSetCursorPos(mainWindow, SCREEN_WIDTH/2, SCREEN_HEIGHT/2); // Compute new orientation horizontalAngle += mouseSpeed * (float)( SCREEN_WIDTH/2 - xpos ); verticalAngle += mouseSpeed * (float)( SCREEN_HEIGHT/2 - ypos ); /* If F1, reload */ if (glfwGetKey(mainWindow, GLFW_KEY_F1)) { Engine_Reload(); } /* If F2, reset view */ if (glfwGetKey(mainWindow, GLFW_KEY_F2)) { printf("Camera reset.\n"); Camera_Reset(); } /* If tab, change lookat to next mesh */ if (glfwGetKey(mainWindow, GLFW_KEY_TAB)) { //Camera_LookAtNext(); } /* Move closer */ //if (glfwGetKey(mainWindow, GLFW_KEY_W)) { // ypos = 100; //} /* Move further */ //if (glfwGetKey(mainWindow, GLFW_KEY_S)) { // ypos = -100; //} /* Move left */ //if (glfwGetKey(mainWindow, GLFW_KEY_A)) { // xpos = 100; //} /* Move right */ //if (glfwGetKey(mainWindow, GLFW_KEY_D)) { // xpos = -100; //} // Compute new orientation //horizontalAngle += mouseSpeed * (float)(1024/2 - xpos ); //verticalAngle += mouseSpeed * (float)( 768/2 - ypos ); horizontalAngle += mouseSpeed * xpos; verticalAngle += mouseSpeed * ypos; //printf("horizontalAngle is %f, and verticalAngle is %f.\n", horizontalAngle, verticalAngle); // Direction : Spherical coordinates to Cartesian coordinates conversion float temp1 = cos(verticalAngle) * sin(horizontalAngle); float temp2 = sin(verticalAngle); float temp3 = cos(verticalAngle) * cos(horizontalAngle); kmVec3 direction = {temp1, temp2, temp3}; // Right vector kmVec3 right = { sin(horizontalAngle - 3.14f/2.0f), 0, cos(horizontalAngle - 3.14f/2.0f) }; // Up vector kmVec3 up = *kmVec3Cross(&up, &right, &direction ); // Move forward if (glfwGetKey( mainWindow, GLFW_KEY_UP ) == GLFW_PRESS){ kmVec3Add(&position, &position, kmVec3Scale(&position, &direction, deltaTime * speed)); } // Move backward if (glfwGetKey( mainWindow, GLFW_KEY_DOWN ) == GLFW_PRESS){ kmVec3Subtract(&position, &position, kmVec3Scale(&position, &direction, deltaTime * speed)); } // Strafe right if (glfwGetKey( mainWindow, GLFW_KEY_RIGHT ) == GLFW_PRESS){ kmVec3Add(&position, &position, kmVec3Scale(&position, &right, deltaTime * speed)); } // Strafe left if (glfwGetKey( mainWindow, GLFW_KEY_LEFT ) == GLFW_PRESS){ kmVec3Subtract(&position, &position, kmVec3Scale(&position, &right, deltaTime * speed)); } float FoV = initialFoV; kmMat4PerspectiveProjection(&ProjectionMatrix, FoV, (16.0f / 8.0f), 0.1f, 100.0f); // Camera matrix //ViewMatrix = glm::lookAt( // position, // Camera is here // position+direction, // and looks here : at the same position, plus "direction" // up // Head is up (set to 0,-1,0 to look upside-down) // ); kmVec3 p_eye; p_eye = position; kmVec3 p_ctr; p_ctr = *kmVec3Add(&p_ctr, &position, &direction); kmVec3 p_up; p_up = up; kmMat4LookAt(&ViewMatrix, &p_eye, &p_ctr, &p_up); kmMat4Identity(&ModelMatrix); kmMat4 temp_mat; kmMat4Multiply(&temp_mat, &ViewMatrix, &ModelMatrix); kmMat4Multiply(&MVP, &ProjectionMatrix, &temp_mat ); //printf("Matrix contains:\n"); //printf("%f, %f, %f, %f\n", MVP.mat[0], MVP.mat[1], MVP.mat[2], MVP.mat[3]); //printf("%f, %f, %f, %f\n", MVP.mat[4], MVP.mat[5], MVP.mat[6], MVP.mat[7]); //printf("%f, %f, %f, %f\n", MVP.mat[8], MVP.mat[9], MVP.mat[10], MVP.mat[11]); //printf("%f, %f, %f, %f\n", MVP.mat[12], MVP.mat[13], MVP.mat[14], MVP.mat[15]); return 0; }