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;
}
