double vectorPointPlaneDistance(struct Vector point1, struct Vector point2, struct Vector normal)
{
normal = vectorNormalize(normal);
double d1 = vectorDot(normal, point1);
double d2 = vectorDot(normal, point2);
return fabs(d1-d2);
}
/* Check if the ray and sphere intersect */
bool intersectRaySphere(ray *r, sphere *s){
/* A = d.d, the vector dot product of the direction */
float A = vectorDot(&r->dir, &r->dir);
/* We need a vector representing the distance between the start of
* the ray and the position of the circle.
* This is the term (p0 - c)
*/
vector dist = vectorSub(&r->start, &s->pos);
/* 2d.(p0 - c) */
float B = 2 * vectorDot(&r->dir, &dist);
/* (p0 - c).(p0 - c) - r^2 */
float C = vectorDot(&dist, &dist) - (s->radius * s->radius);
/* Solving the discriminant */
float discr = B * B - 4 * A * C;
/* If the discriminant is negative, there are no real roots.
* Return false in that case as the ray misses the sphere.
* Return true in all other cases (can be one or two intersections)
*/
if(discr < 0)
return false;
else
return true;
}
float MeshShape::calculateMomentOfInertia(float *rotationvector) {
if (vectorDot(rotationvector, rotationvector) < EPSILON)
return 0;
int i;
float j = 0;
for (i = 0; i < mesh->vertexcount; i++) {
float proj[3];
vectorProject(proj, mesh->vertices[i].position, rotationvector);
vectorSub(proj, mesh->vertices[i].position, proj);
// float r = vectorLength(proj);
float r2 = vectorDot(proj, proj);
j += r2;
}
return j / i;
}
Vector Biot_Savart_SingleLineSeg(LineSeg seg,Vector fieldpoint)
{
Vector result,r,Seg_Cross_r;
Vector SegMidPoint,SegVector,SegDirection;
Vector BDirection;
double SegLength,x,z,constval;
double Bmag;
SegMidPoint=linesegCenter(seg);
SegLength=linesegLength(seg);
SegVector=linesegVector(seg);
SegDirection=vectorUnitVector(SegVector);
r=linesegVector(linesegGenerate(SegMidPoint,fieldpoint));
Seg_Cross_r=vectorCross(SegVector,r);
BDirection=vectorUnitVector(Seg_Cross_r);
z=vectorMagnitude(Seg_Cross_r)/SegLength;
constval=mu_0/(4*M_PI*z);
x=vectorDot(SegDirection,r);
if(z == 0)
{
Bmag=0;
}
else
{
Bmag=constval*((SegLength-2*x)/sqrt((SegLength-2*x)*(SegLength-2*x)+4*z*z)+(SegLength+2*x)/sqrt((SegLength+2*x)*(SegLength+2*x)+4*z*z));
}
//printf("Bmag %g\n",Bmag);
result=vectorScale(BDirection,Bmag);
//printf("result.x,result.y,result.z %g,%g,%g\n",result.x,result.y,result.z);
return result;
}
void MagneticSensorLsm303::vectorNormalize(vector<float> *a)
{
float mag = sqrt(vectorDot(a, a));
a->x /= mag;
a->y /= mag;
a->z /= mag;
}
/* Returns Positive on separation - gives distance*/
double boundingBoxSeparationTest(struct Vector L, struct BoundingBox a, struct BoundingBox b)
{
double R = fabs(vectorDot(L, vectorSub(a.position, b.position)));
double R0 = a.halfSize.x*fabs(vectorDot(L, a.direction)) +
a.halfSize.y*fabs(vectorDot(L, a.up)) +
a.halfSize.z*fabs(vectorDot(L, vectorCross(a.up, a.direction)));
double R1 = b.halfSize.x*fabs(vectorDot(L, b.direction)) +
b.halfSize.y*fabs(vectorDot(L, b.up)) +
b.halfSize.z*fabs(vectorDot(L, vectorCross(b.up, b.direction)));
return R - (R0 + R1);
}
/* Check if a ray collides with a sphere */
bool collideRaySphere(ray *r, sphere *s, double *t)
{
/* Ray/Sphere intersection
*
* collideRaySpere(ray,sphere)
* Vector distance = ray.start - sphere.position
* B = distance dot ray.direction
* C = distance dot distance - sphere.radius^2
* Discriminant = B^2 - C
* Discriminant < 0 --> no collision
* Otherwise select closest solution
*/
vector dist = vectorSub(&r->start, &s->pos);
double B = vectorDot(&dist, &r->dir);
double D = B * B - vectorDot(&dist, &dist) + s->size * s->size;
/* Discriminant less than 0 ? */
if (D < 0.0f)
return FALSE;
double t0 = -B - sqrtf(D);
double t1 = -B + sqrtf(D);
bool retvalue = FALSE;
if ((t0 > 0.1f) && (t0 < *t)) {
*t = t0;
retvalue = TRUE;
}
if ((t1 > 0.1f) && (t1 < *t)) {
*t = t1;
retvalue = TRUE;
}
return retvalue;
}
template <typename T> float MagneticSensorLsm303::heading(vector<T> from)
{
vector<int32_t> temp_m = {magnetometer.x, magnetometer.y, magnetometer.z};
///< subtract offset (average of min and max) from magnetometer readings
temp_m.x -= ((int32_t)magnetometer_min.x + magnetometer_max.x) / 2;
temp_m.y -= ((int32_t)magnetometer_min.y + magnetometer_max.y) / 2;
temp_m.z -= ((int32_t)magnetometer_min.z + magnetometer_max.z) / 2;
///< compute E and N
vector<float> E;
vector<float> N;
vectorCross(&temp_m, &accelerometer, &E);
vectorNormalize(&E);
vectorCross(&accelerometer, &E, &N);
vectorNormalize(&N);
///< compute heading
float heading = atan2(vectorDot(&E, &from), vectorDot(&N, &from)) * 180 / M_PI;
if (heading < 0) heading += 360;
return heading;
}
/* Not really the best or most accurate way, but it works... */
double AutoExposure(scene *myScene)
{
#define ACCUMULATION_SIZE 256
double exposure = -1.0f;
double accumulationfactor = (double)max(myScene->width, myScene->height);
double projectionDistance = 0.0f;
if ((myScene->persp.type == CONIC) && myScene->persp.FOV > 0.0f && myScene->persp.FOV < 189.0f) {
projectionDistance = 0.5f * myScene->width / tanf ((double)(PIOVER180) * 0.5f * myScene->persp.FOV);
}
accumulationfactor = accumulationfactor / ACCUMULATION_SIZE;
colour mediumPoint = {0.0f, 0.0f, 0.0f};
const double mediumPointWeight = 1.0f / (ACCUMULATION_SIZE*ACCUMULATION_SIZE);
int x,y;
for (y = 0; y < ACCUMULATION_SIZE; ++y) {
for (x = 0 ; x < ACCUMULATION_SIZE; ++x) {
if (myScene->persp.type == ORTHOGONAL || projectionDistance == 0.0f) {
ray viewRay = { {(double)x*accumulationfactor, (double)y * accumulationfactor, -10000.0f}, { 0.0f, 0.0f, 1.0f}};
colour currentColor = raytrace(&viewRay, myScene);
colour tmp = colourMul(¤tColor, ¤tColor);
tmp = colourCoefMul(mediumPointWeight, &tmp);
mediumPoint = colourAdd(&mediumPoint,&tmp);
} else {
vector dir = {((double)(x)*accumulationfactor - 0.5f * myScene->width) / projectionDistance,
((double)(y)*accumulationfactor - 0.5f * myScene->height) / projectionDistance,
1.0f
};
double norm = vectorDot(&dir,&dir);
if (norm == 0.0f)
break;
dir = vectorScale(invsqrtf(norm), &dir);
ray viewRay = { {0.5f * myScene->width, 0.5f * myScene->height, 0.0f}, {dir.x, dir.y, dir.z} };
colour currentColor = raytrace(&viewRay, myScene);
colour tmp = colourMul(¤tColor, ¤tColor);
tmp = colourCoefMul(mediumPointWeight, &tmp);
mediumPoint = colourAdd(&mediumPoint,&tmp);
}
}
}
double mediumLuminance = sqrtf(0.2126f * mediumPoint.red + 0.715160f * mediumPoint.green + 0.072169f * mediumPoint.blue);
if (mediumLuminance > 0.001f) {
// put the medium luminance to an intermediate gray value
exposure = logf(0.6f) / mediumLuminance;
}
return exposure;
}
bool tracePlane(tracehit *result, float *origin, float *ray,
class Polygon *polygon) {
float *normal = polygon->planenormal;
float D = polygon->planedistance;
float denominator = vectorDot(normal, ray);
if (denominator == 0) {
if (vectorDot(normal, origin) > 0)
result->t = 1000000;
else
result->t = -1000000;
result->polygon = polygon;
return true;
}
float t = -(vectorDot(normal, origin) + D) / denominator;
result->t = t;
result->polygon = polygon;
// if (t < 0 || t > 1) return false;
return true;
}
void Mesh::createPlanes(void) {
int i;
for (i = 0; i < polygoncount; i++) {
class Polygon *polygon = &this->polygons[i];
if (polygon->vertexcount >= 3) {
float v1[3], v2[3];
vectorSub(v1, polygon->vertices[1]->position,
polygon->vertices[0]->position);
vectorSub(v2, polygon->vertices[2]->position,
polygon->vertices[0]->position);
vectorCross(polygon->planenormal, v1, v2);
vectorNormalize(polygon->planenormal);
polygon->planedistance =
-vectorDot(polygon->vertices[0]->position, polygon->planenormal);
}
}
}
bool checkEdgeMeshCollision(float *p1, float *p2, Mesh *mesh, float *normal,
float *contactpoint) {
float ray[3];
vectorSub(ray, p2, p1);
// UNUSED//float maxdist = 0;
// UNUSED//bool collision = false;
int i, j;
tracehit hits[MAXPOLYGONS];
int hitcount = 0;
for (i = 0; i < mesh->polygoncount; i++) {
class Polygon *polygon = &mesh->polygons[i];
if (tracePlane(&hits[hitcount], p1, ray, polygon)) {
hitcount++;
}
}
if (hitcount < 2)
return false;
for (i = 1; i < hitcount; i++) {
for (j = i; j > 0; j--) {
if (hits[j].t < hits[j - 1].t) {
float tempt = hits[j].t;
hits[j].t = hits[j - 1].t;
hits[j - 1].t = tempt;
class Polygon *tempp = hits[j].polygon;
hits[j].polygon = hits[j - 1].polygon;
hits[j - 1].polygon = tempp;
} else
break;
}
}
int negative = -1, positive = -1;
for (i = 0; i < hitcount; i++) {
// UNUSED//float t = hits[i].t;
class Polygon *polygon = hits[i].polygon;
float dot = vectorDot(ray, polygon->planenormal);
if (dot > 0 && positive == -1)
positive = i;
if (dot < 0)
negative = i;
if (dot < 0 && positive != -1)
return false;
}
if (negative == -1 || positive == -1)
return false;
/*for (i = 0; i < hitcount; i++){
float t = hits[i].t;
class Polygon *polygon = hits[i].polygon;
float dot = vectorDot(ray, polygon->planenormal);
printf("%f ", dot);
}
printf("\n");*/
if (hits[negative].t < 0 || hits[positive].t > 1)
return false;
Edge *edge2 = findSharingEdge(hits[negative].polygon, hits[positive].polygon);
// fflush(stdout);
float cp1[3], cp2[3];
vectorScale(cp1, ray, hits[negative].t);
vectorAdd(cp1, p1);
vectorScale(cp2, ray, hits[positive].t);
vectorAdd(cp2, p1);
if (edge2 != NULL) {
/*float ev1[3];
vectorSub(ev1, edge2->v2->position, edge2->v1->position);
vectorCross(normal, ev1, ray);
vectorScale(normal, vectorDot(normal, hits[positive].polygon->planenormal));
vectorNormalize(normal);
float at = (hits[negative].t + hits[positive].t) / 2;
vectorScale(contactpoint, ray, at);
vectorAdd(contactpoint, p1);*/
float dot1 = fabs(vectorDot(ray, hits[negative].polygon->planenormal));
float dot2 = fabs(vectorDot(ray, hits[positive].polygon->planenormal));
if (dot1 > dot2) {
// vectorScale(contactpoint, ray, hits[negative].t);
// vectorAdd(contactpoint, p1);
vectorCopy(contactpoint, cp1);
vectorCopy(normal, hits[positive].polygon->planenormal);
} else {
// vectorScale(contactpoint, ray, hits[positive].t);
// vectorAdd(contactpoint, p1);
vectorCopy(contactpoint, cp2);
vectorCopy(normal, hits[negative].polygon->planenormal);
}
} else {
Polygon *polygon = findNearestPolygon(hits[negative].polygon, cp1);
if (polygon != NULL) {
/*vectorCopy(contactpoint, cp1);
vectorAdd(contactpoint, cp2);
vectorScale(contactpoint, 0.5);*/
float at = (hits[negative].t + hits[positive].t) / 2;
vectorScale(contactpoint, ray, at);
vectorAdd(contactpoint, p1);
vectorCopy(normal, polygon->planenormal);
} else {
return false;
}
}
// shotsound->play();
return true;
}
/***************************************
* Conjugate Gradient *
* This function will do the CG *
* algorithm without preconditioning. *
* For optimiziation you must not *
* change the algorithm. *
***************************************
r(0) = b - Ax(0)
p(0) = r(0)
rho(0) = <r(0),r(0)>
***************************************
for k=0,1,2,...,n-1
q(k) = A * p(k)
dot_pq = <p(k),q(k)>
alpha = rho(k) / dot_pq
x(k+1) = x(k) + alpha*p(k)
r(k+1) = r(k) - alpha*q(k)
check convergence ||r(k+1)||_2 < eps
rho(k+1) = <r(k+1), r(k+1)>
beta = rho(k+1) / rho(k)
p(k+1) = r(k+1) + beta*p(k)
***************************************/
void cg(const int n, const int nnz, const int maxNNZ, const floatType* data, const int* indices, const int* length, const floatType* b, floatType* x, struct SolverConfig* sc){
floatType* r, *p, *q;
floatType alpha, beta, rho, rho_old, dot_pq, bnrm2;
int iter;
double timeMatvec_s;
double timeMatvec=0;
int i;
floatType temp;
/* allocate memory */
r = (floatType*)malloc(n * sizeof(floatType));
p = (floatType*)malloc(n * sizeof(floatType));
q = (floatType*)malloc(n * sizeof(floatType));
#pragma acc data copyin(data[0:n*maxNNZ], indices[0:n*maxNNZ], length[0:n], n, nnz, maxNNZ, b[0:n]) copy(x[0:n]) create(alpha, beta, r[0:n], p[0:n], q[0:n], i, temp) //eigentlich auch copy(x[0:n]) aber error: not found on device???
{
DBGMAT("Start matrix A = ", n, nnz, maxNNZ, data, indices, length)
DBGVEC("b = ", b, n);
DBGVEC("x = ", x, n);
/* r(0) = b - Ax(0) */
timeMatvec_s = getWTime();
matvec(n, nnz, maxNNZ, data, indices, length, x, r);
//hier inline ausprobieren
/*int i, j, k;
#pragma acc parallel loop present(data, indices, length, x)
for (i = 0; i < n; i++) {
r[i] = 0;
for (j = 0; j < length[i]; j++) {
k = j * n + i;
r[i] += data[k] * x[indices[k]];
}
}*/
timeMatvec += getWTime() - timeMatvec_s;
xpay(b, -1.0, n, r);
DBGVEC("r = b - Ax = ", r, n);
/* Calculate initial residuum */
nrm2(r, n, &bnrm2);
bnrm2 = 1.0 /bnrm2;
/* p(0) = r(0) */
memcpy(p, r, n*sizeof(floatType));
DBGVEC("p = r = ", p, n);
/* rho(0) = <r(0),r(0)> */
vectorDot(r, r, n, &rho);
printf("rho_0=%e\n", rho);
for(iter = 0; iter < sc->maxIter; iter++){
DBGMSG("=============== Iteration %d ======================\n", iter);
/* q(k) = A * p(k) */
timeMatvec_s = getWTime();
matvec(n, nnz, maxNNZ, data, indices, length, p, q);
timeMatvec += getWTime() - timeMatvec_s;
DBGVEC("q = A * p= ", q, n);
/* dot_pq = <p(k),q(k)> */
vectorDot(p, q, n, &dot_pq);
DBGSCA("dot_pq = <p, q> = ", dot_pq);
/* alpha = rho(k) / dot_pq */
alpha = rho / dot_pq;
DBGSCA("alpha = rho / dot_pq = ", alpha);
/* x(k+1) = x(k) + alpha*p(k) */
axpy(alpha, p, n, x);
#pragma acc update host(x[0:n])
DBGVEC("x = x + alpha * p= ", x, n);
/* r(k+1) = r(k) - alpha*q(k) */
axpy(-alpha, q, n, r);
DBGVEC("r = r - alpha * q= ", r, n);
rho_old = rho;
DBGSCA("rho_old = rho = ", rho_old);
/* rho(k+1) = <r(k+1), r(k+1)> */
vectorDot(r, r, n, &rho);
DBGSCA("rho = <r, r> = ", rho);
/* Normalize the residual with initial one */
sc->residual= sqrt(rho) * bnrm2;
/* Check convergence ||r(k+1)||_2 < eps
* If the residual is smaller than the CG
* tolerance specified in the CG_TOLERANCE
* environment variable our solution vector
* is good enough and we can stop the
* algorithm. */
printf("res_%d=%e\n", iter+1, sc->residual);
if(sc->residual <= sc->tolerance)
break;
/* beta = rho(k+1) / rho(k) */
beta = rho / rho_old;
DBGSCA("beta = rho / rho_old= ", beta);
/* p(k+1) = r(k+1) + beta*p(k) */
xpay(r, beta, n, p);
DBGVEC("p = r + beta * p> = ", p, n);
}
/* Store the number of iterations and the
* time for the sparse matrix vector
* product which is the most expensive
* function in the whole CG algorithm. */
sc->iter = iter;
sc->timeMatvec = timeMatvec;
/* Clean up */
free(r);
free(p);
free(q);
}//ende data region
}
void boundingBoxCollisionPoint(struct Vector* normal, struct Vector* point, struct BoundingBox ai, struct BoundingBox bi, struct BoundingBox af, struct BoundingBox bf)
{
int i;
int separartingAxesNum = 0;
int lastSeparartingAxis = 0;
struct BoundingBox a;
struct BoundingBox b;
double distance, minDistance;
i=1000;
while(separartingAxesNum != 1 && i-->0)
{
separartingAxesNum = 0;
a = boundingBoxInterpolate(&ai, &af);
b = boundingBoxInterpolate(&bi, &bf);
struct Vector a1 = a.direction = vectorNormalize(a.direction);
struct Vector a2 = a.up = vectorNormalize(a.up);
struct Vector a3 = vectorCross(a1, a2);
struct Vector b1 = b.direction = vectorNormalize(b.direction);
struct Vector b2 = b.up = vectorNormalize(b.up);
struct Vector b3 = vectorCross(b1, b2);
if(boundingBoxSeparationTest(a1, a, b)>0) {separartingAxesNum++; lastSeparartingAxis=0;}
if(boundingBoxSeparationTest(a2, a, b)>0) {separartingAxesNum++; lastSeparartingAxis=1;}
if(boundingBoxSeparationTest(a3, a, b)>0) {separartingAxesNum++; lastSeparartingAxis=2;}
if(boundingBoxSeparationTest(b1, a, b)>0) {separartingAxesNum++; lastSeparartingAxis=3;}
if(boundingBoxSeparationTest(b2, a, b)>0) {separartingAxesNum++; lastSeparartingAxis=4;}
if(boundingBoxSeparationTest(b3, a, b)>0) {separartingAxesNum++; lastSeparartingAxis=5;}
if(boundingBoxSeparationTest(vectorCross(a1, b1), a, b)>0) {separartingAxesNum++; lastSeparartingAxis=6;}
if(boundingBoxSeparationTest(vectorCross(a1, b2), a, b)>0) {separartingAxesNum++; lastSeparartingAxis=7;}
if(boundingBoxSeparationTest(vectorCross(a1, b3), a, b)>0) {separartingAxesNum++; lastSeparartingAxis=8;}
if(boundingBoxSeparationTest(vectorCross(a2, b1), a, b)>0) {separartingAxesNum++; lastSeparartingAxis=9;}
if(boundingBoxSeparationTest(vectorCross(a2, b2), a, b)>0) {separartingAxesNum++; lastSeparartingAxis=10;}
if(boundingBoxSeparationTest(vectorCross(a2, b3), a, b)>0) {separartingAxesNum++; lastSeparartingAxis=11;}
if(boundingBoxSeparationTest(vectorCross(a3, b1), a, b)>0) {separartingAxesNum++; lastSeparartingAxis=12;}
if(boundingBoxSeparationTest(vectorCross(a3, b2), a, b)>0) {separartingAxesNum++; lastSeparartingAxis=13;}
if(boundingBoxSeparationTest(vectorCross(a3, b3), a, b)>0) {separartingAxesNum++; lastSeparartingAxis=14;}
if(separartingAxesNum>1) // is not colliding yet
{
ai = a;
bi = b;
}
if(separartingAxesNum<1) // is already colliding
{
af = a;
bf = b;
}
}
if(lastSeparartingAxis<6) // face-vertex
{
struct Vector center;
int pointId = 0;
struct Vector verticles[6];
if(lastSeparartingAxis<3) // b-vertex collide with a-face
{
boundingBoxGetVerticles(verticles, &b);
center = a.position;
}
else // a-vertex collide with b-face
{
boundingBoxGetVerticles(verticles, &a);
center = b.position;
}
switch(lastSeparartingAxis)
{
case 0:
*normal = a.direction;
distance = a.halfSize.x;
break;
case 1:
*normal = a.up;
distance = a.halfSize.y;
break;
case 2:
*normal = vectorCross(a.direction, a.up);
distance = a.halfSize.z;
break;
case 3:
*normal = b.direction;
distance = b.halfSize.x;
break;
case 4:
*normal = b.up;
distance = b.halfSize.y;
break;
case 5:
*normal = vectorCross(b.direction, b.up);;
distance = b.halfSize.z;
break;
default:
*normal = vectorZero();
distance = 0;
}
minDistance = -1;
for(i=0;i<6;i++)
{
distance = vectorPointPlaneDistance(verticles[i], center, *normal);
if(minDistance < 0 || distance < minDistance)
{
minDistance = distance;
pointId = i;
}
}
*point = verticles[pointId];
}
else // edge-edge
{
struct Vector a1 = a.direction = vectorNormalize(a.direction);
struct Vector a2 = a.up = vectorNormalize(a.up);
struct Vector a3 = vectorNormalize( vectorCross(a1, a2) );
struct Vector b1 = b.direction = vectorNormalize(b.direction);
struct Vector b2 = b.up = vectorNormalize(b.up);
struct Vector b3 = vectorNormalize(vectorCross(b1, b2));
struct Vector halfEdgeA, halfEdgeB;
struct Vector halfOffsetA[4];
struct Vector halfOffsetB[4];
for(i=0;i<4;i++)
{switch(lastSeparartingAxis)
{
case 6: case 7: case 8:
halfEdgeA = vectorTimes(a1, a.halfSize.x);
halfOffsetA[i] = vectorAdd( vectorTimes(a1, 0),
vectorAdd( vectorTimes(a2, a.halfSize.y * (1-2*(i%2)) ),
vectorTimes(a3, a.halfSize.z * (i>=2 ? -1 : 1) )));
break;
case 9: case 10: case 11:
halfEdgeA = vectorTimes(a2, a.halfSize.y);
halfOffsetA[i] = vectorAdd( vectorTimes(a1, a.halfSize.x * (1-2*(i%2))),
vectorAdd( vectorTimes(a2, 0),
vectorTimes(a3, a.halfSize.z * (i>=2 ? -1 : 1))));
break;
case 12: case 13: case 14:
halfEdgeA = vectorTimes(a3, a.halfSize.z);
halfOffsetA[i] = vectorAdd( vectorTimes(a1, a.halfSize.x * (1-2*(i%2))),
vectorAdd( vectorTimes(a2, a.halfSize.y * (i>=2 ? -1 : 1)),
vectorTimes(a3, 0)));
break;
}
switch(lastSeparartingAxis)
{
case 6: case 9: case 12:
halfEdgeB = vectorTimes(b1, b.halfSize.x);
halfOffsetB[i] = vectorAdd( vectorTimes(b1, 0),
vectorAdd( vectorTimes(b2, b.halfSize.y * (1-2*(i%2))),
vectorTimes(b3, b.halfSize.z * (i>=2 ? -1 : 1))));
break;
case 7: case 10: case 13:
halfEdgeB = vectorTimes(b2, b.halfSize.y);
halfOffsetB[i] = vectorAdd( vectorTimes(b1, b.halfSize.x * (1-2*(i%2))),
vectorAdd( vectorTimes(b2, 0),
vectorTimes(b3, b.halfSize.z * (i>=2 ? -1 : 1))));
break;
case 8: case 11: case 14:
halfEdgeB = vectorTimes(b3, b.halfSize.z);
halfOffsetB[i] = vectorAdd( vectorTimes(b1, b.halfSize.x * (1-2*(i%2))),
vectorAdd( vectorTimes(b2, b.halfSize.y * (i>=2 ? -1 : 1)),
vectorTimes(b3, 0)));
break;
}
}
*normal = vectorNormalize( vectorCross(halfEdgeA, halfEdgeB) );
int edgeAi, edgeBi;
int edgeA, edgeB;
minDistance = -1;
for(edgeAi = 0; edgeAi<4; edgeAi++)
for(edgeBi = 0; edgeBi<4; edgeBi++)
{
distance = vectorDot(*normal, vectorAdd(a.position, halfOffsetA[edgeAi])) - vectorDot(*normal, vectorAdd(b.position, halfOffsetB[edgeBi]));
distance = fabs(distance);
if(minDistance == -1 || distance < minDistance)
{
minDistance = distance;
edgeA = edgeAi;
edgeB = edgeBi;
}
}
struct Vector pointEdgeA = vectorAdd(a.position, halfOffsetA[edgeA]);
struct Vector pointEdgeB = vectorAdd(b.position, halfOffsetB[edgeB]);
struct Vector rayVector = vectorNormalize(halfEdgeA);
struct Vector rayPoint = pointEdgeA;
struct Vector planePoint = pointEdgeB;
struct Vector planeNormal = vectorNormalize(vectorCross(vectorNormalize(halfEdgeB), *normal));
// ray - plane intersection
double cosAlpha = vectorDot(rayVector, planeNormal);
double distance = vectorPointPlaneDistance(rayPoint, planePoint, planeNormal);
struct Vector pointA = vectorAdd(rayPoint, vectorTimes(rayVector, cosAlpha*distance)) ;
rayVector = vectorNormalize(halfEdgeB);
rayPoint = pointEdgeB;
planePoint = pointEdgeA;
planeNormal = vectorNormalize(vectorCross(vectorNormalize(halfEdgeA), *normal));
// ray - plane intersection
cosAlpha = vectorDot(rayVector, planeNormal);
distance = vectorPointPlaneDistance(rayPoint, planePoint, planeNormal);
struct Vector pointB = vectorAdd(rayPoint, vectorTimes(rayVector, cosAlpha*distance)) ;
*point = vectorTimes(vectorAdd(pointA, pointB),0.5);
}
/* normal should point in direction from A to B*/
double smallValue = 0.001;
if(vectorLength(vectorSub(vectorAdd(*point,vectorTimes(*normal, smallValue)), a.position))
<
vectorLength(vectorSub(vectorAdd(*point,vectorZero()), a.position))) /* |point + normal - A| < |point-A| */
*normal = vectorTimes(*normal, -1.0);
*normal = vectorNormalize(*normal);
}
/**
* Solves a linear least squares problem to obtain a N degree polynomial that fits
* the specified input data as nearly as possible.
*
* Returns true if a solution is found, false otherwise.
*
* The input consists of two vectors of data points X and Y with indices 0..m-1
* along with a weight vector W of the same size.
*
* The output is a vector B with indices 0..n that describes a polynomial
* that fits the data, such the sum of W[i] * W[i] * abs(Y[i] - (B[0] + B[1] X[i]
* + B[2] X[i]^2 ... B[n] X[i]^n)) for all i between 0 and m-1 is minimized.
*
* Accordingly, the weight vector W should be initialized by the caller with the
* reciprocal square root of the variance of the error in each input data point.
* In other words, an ideal choice for W would be W[i] = 1 / var(Y[i]) = 1 / stddev(Y[i]).
* The weights express the relative importance of each data point. If the weights are
* all 1, then the data points are considered to be of equal importance when fitting
* the polynomial. It is a good idea to choose weights that diminish the importance
* of data points that may have higher than usual error margins.
*
* Errors among data points are assumed to be independent. W is represented here
* as a vector although in the literature it is typically taken to be a diagonal matrix.
*
* That is to say, the function that generated the input data can be approximated
* by y(x) ~= B[0] + B[1] x + B[2] x^2 + ... + B[n] x^n.
*
* The coefficient of determination (R^2) is also returned to describe the goodness
* of fit of the model for the given data. It is a value between 0 and 1, where 1
* indicates perfect correspondence.
*
* This function first expands the X vector to a m by n matrix A such that
* A[i][0] = 1, A[i][1] = X[i], A[i][2] = X[i]^2, ..., A[i][n] = X[i]^n, then
* multiplies it by w[i]./
*
* Then it calculates the QR decomposition of A yielding an m by m orthonormal matrix Q
* and an m by n upper triangular matrix R. Because R is upper triangular (lower
* part is all zeroes), we can simplify the decomposition into an m by n matrix
* Q1 and a n by n matrix R1 such that A = Q1 R1.
*
* Finally we solve the system of linear equations given by R1 B = (Qtranspose W Y)
* to find B.
*
* For efficiency, we lay out A and Q column-wise in memory because we frequently
* operate on the column vectors. Conversely, we lay out R row-wise.
*
* http://en.wikipedia.org/wiki/Numerical_methods_for_linear_least_squares
* http://en.wikipedia.org/wiki/Gram-Schmidt
*/
static bool solveLeastSquares(const float* x, const float* y,
const float* w, uint32_t m, uint32_t n, float* outB, float* outDet) {
#if DEBUG_STRATEGY
ALOGD("solveLeastSquares: m=%d, n=%d, x=%s, y=%s, w=%s", int(m), int(n),
vectorToString(x, m).c_str(), vectorToString(y, m).c_str(),
vectorToString(w, m).c_str());
#endif
// Expand the X vector to a matrix A, pre-multiplied by the weights.
float a[n][m]; // column-major order
for (uint32_t h = 0; h < m; h++) {
a[0][h] = w[h];
for (uint32_t i = 1; i < n; i++) {
a[i][h] = a[i - 1][h] * x[h];
}
}
#if DEBUG_STRATEGY
ALOGD(" - a=%s", matrixToString(&a[0][0], m, n, false /*rowMajor*/).c_str());
#endif
// Apply the Gram-Schmidt process to A to obtain its QR decomposition.
float q[n][m]; // orthonormal basis, column-major order
float r[n][n]; // upper triangular matrix, row-major order
for (uint32_t j = 0; j < n; j++) {
for (uint32_t h = 0; h < m; h++) {
q[j][h] = a[j][h];
}
for (uint32_t i = 0; i < j; i++) {
float dot = vectorDot(&q[j][0], &q[i][0], m);
for (uint32_t h = 0; h < m; h++) {
q[j][h] -= dot * q[i][h];
}
}
float norm = vectorNorm(&q[j][0], m);
if (norm < 0.000001f) {
// vectors are linearly dependent or zero so no solution
#if DEBUG_STRATEGY
ALOGD(" - no solution, norm=%f", norm);
#endif
return false;
}
float invNorm = 1.0f / norm;
for (uint32_t h = 0; h < m; h++) {
q[j][h] *= invNorm;
}
for (uint32_t i = 0; i < n; i++) {
r[j][i] = i < j ? 0 : vectorDot(&q[j][0], &a[i][0], m);
}
}
#if DEBUG_STRATEGY
ALOGD(" - q=%s", matrixToString(&q[0][0], m, n, false /*rowMajor*/).c_str());
ALOGD(" - r=%s", matrixToString(&r[0][0], n, n, true /*rowMajor*/).c_str());
// calculate QR, if we factored A correctly then QR should equal A
float qr[n][m];
for (uint32_t h = 0; h < m; h++) {
for (uint32_t i = 0; i < n; i++) {
qr[i][h] = 0;
for (uint32_t j = 0; j < n; j++) {
qr[i][h] += q[j][h] * r[j][i];
}
}
}
ALOGD(" - qr=%s", matrixToString(&qr[0][0], m, n, false /*rowMajor*/).c_str());
#endif
// Solve R B = Qt W Y to find B. This is easy because R is upper triangular.
// We just work from bottom-right to top-left calculating B's coefficients.
float wy[m];
for (uint32_t h = 0; h < m; h++) {
wy[h] = y[h] * w[h];
}
for (uint32_t i = n; i != 0; ) {
i--;
outB[i] = vectorDot(&q[i][0], wy, m);
for (uint32_t j = n - 1; j > i; j--) {
outB[i] -= r[i][j] * outB[j];
}
outB[i] /= r[i][i];
}
#if DEBUG_STRATEGY
ALOGD(" - b=%s", vectorToString(outB, n).c_str());
#endif
// Calculate the coefficient of determination as 1 - (SSerr / SStot) where
// SSerr is the residual sum of squares (variance of the error),
// and SStot is the total sum of squares (variance of the data) where each
// has been weighted.
float ymean = 0;
for (uint32_t h = 0; h < m; h++) {
ymean += y[h];
}
ymean /= m;
float sserr = 0;
float sstot = 0;
for (uint32_t h = 0; h < m; h++) {
float err = y[h] - outB[0];
float term = 1;
for (uint32_t i = 1; i < n; i++) {
term *= x[h];
err -= term * outB[i];
}
sserr += w[h] * w[h] * err * err;
float var = y[h] - ymean;
sstot += w[h] * w[h] * var * var;
}
*outDet = sstot > 0.000001f ? 1.0f - (sserr / sstot) : 1;
#if DEBUG_STRATEGY
ALOGD(" - sserr=%f", sserr);
ALOGD(" - sstot=%f", sstot);
ALOGD(" - det=%f", *outDet);
#endif
return true;
}
void *renderThread(void *arg)
{
int x,y;
thread_info *tinfo = (thread_info *)arg;
/* Calculate which part to render based on thread id */
int limits[]={(tinfo->thread_num*sectionsize),(tinfo->thread_num*sectionsize)+sectionsize};
/* Autoexposure */
double exposure = AutoExposure(myScene);
double projectionDistance = 0.0f;
if ((myScene->persp.type == CONIC) && myScene->persp.FOV > 0.0f && myScene->persp.FOV < 189.0f) {
projectionDistance = 0.5f * myScene->width / tanf((double)(PIOVER180) * 0.5f * myScene->persp.FOV);
}
for (y = limits[0]; y < limits[1]; ++y) {
for (x = 0; x < myScene->width; ++x) {
colour output = {0.0f, 0.0f, 0.0f};
double fragmentx, fragmenty;
/* Antialiasing */
for (fragmentx = x; fragmentx < x + 1.0f; fragmentx += 0.5f) {
for (fragmenty = y; fragmenty < y + 1.0f; fragmenty += 0.5f) {
double sampleRatio=0.25f;
colour temp = {0.0f, 0.0f, 0.0f};
double totalWeight = 0.0f;
if (myScene->persp.type == ORTHOGONAL || projectionDistance == 0.0f) {
ray viewRay = { {fragmentx, fragmenty, -10000.0f},{ 0.0f, 0.0f, 1.0f}};
int i;
for (i = 0; i < myScene->complexity; ++i) {
colour result = raytrace(&viewRay, myScene);
totalWeight += 1.0f;
temp = colourAdd(&temp,&result);
}
temp = colourCoefMul((1.0f/totalWeight), &temp);
} else {
vector dir = {(fragmentx - 0.5f * myScene->width) / projectionDistance,
(fragmenty - 0.5f * myScene->height) / projectionDistance,
1.0f
};
double norm = vectorDot(&dir,&dir);
if (norm == 0.0f)
break;
dir = vectorScale(invsqrtf(norm),&dir);
vector start = {0.5f * myScene->width, 0.5f * myScene->height, 0.0f};
vector tmp = vectorScale(myScene->persp.clearPoint,&dir);
vector observed = vectorAdd(&start,&tmp);
int i;
for (i = 0; i < myScene->complexity; ++i) {
ray viewRay = { {start.x, start.y, start.z}, {dir.x, dir.y, dir.z} };
if (myScene->persp.dispersion != 0.0f) {
vector disturbance;
disturbance.x = (myScene->persp.dispersion / RAND_MAX) * (1.0f * rand());
disturbance.y = (myScene->persp.dispersion / RAND_MAX) * (1.0f * rand());
disturbance.z = 0.0f;
viewRay.start = vectorAdd(&viewRay.start, &disturbance);
viewRay.dir = vectorSub(&observed, &viewRay.start);
norm = vectorDot(&viewRay.dir,&viewRay.dir);
if (norm == 0.0f)
break;
viewRay.dir = vectorScale(invsqrtf(norm),&viewRay.dir);
}
colour result = raytrace(&viewRay, myScene);
totalWeight += 1.0f;
temp = colourAdd(&temp,&result);
}
temp = colourCoefMul((1.0f/totalWeight), &temp);
}
temp.blue = (1.0f - expf(temp.blue * exposure));
temp.red = (1.0f - expf(temp.red * exposure));
temp.green = (1.0f - expf(temp.green * exposure));
colour adjusted = colourCoefMul(sampleRatio, &temp);
output = colourAdd(&output, &adjusted);
}
}
/* gamma correction */
double invgamma = 0.45f; //Fixed value from sRGB standard
output.blue = powf(output.blue, invgamma);
output.red = powf(output.red, invgamma);
output.green = powf(output.green, invgamma);
img[(x+y*myScene->width)*3+2] = (unsigned char)min(output.red*255.0f, 255.0f);
img[(x+y*myScene->width)*3+1] = (unsigned char)min(output.green*255.0f, 255.0f);
img[(x+y*myScene->width)*3+0] = (unsigned char)min(output.blue*255.0f,255.0f);
}
}
pthread_exit((void *) arg);
}
colour raytrace(ray *viewRay, scene *myScene)
{
colour output = {0.0f, 0.0f, 0.0f};
double coef = 1.0f;
int level = 0;
do {
vector hitpoint,n;
int currentSphere = -1;
int currentTriangle = -1;
material currentMat;
double t = 20000.0f;
double temp;
vector n1;
unsigned int i;
for (i = 0; i < myScene->numSpheres; ++i) {
if (collideRaySphere(viewRay, &myScene->spheres[i], &t)) {
currentSphere = i;
}
}
for (i = 0; i < myScene->numTriangles; ++i) {
if (collideRayTriangle(viewRay, &myScene->triangles[i], &t, &n1)) {
currentTriangle = i;
currentSphere = -1;
}
}
if (currentSphere != -1) {
vector scaled = vectorScale(t, &viewRay->dir);
hitpoint = vectorAdd(&viewRay->start, &scaled);
n = vectorSub(&hitpoint, &myScene->spheres[currentSphere].pos);
temp = vectorDot(&n,&n);
if (temp == 0.0f) break;
temp = invsqrtf(temp);
n = vectorScale(temp, &n);
currentMat = myScene->materials[myScene->spheres[currentSphere].material];
} else if (currentTriangle != -1) {
vector scaled = vectorScale(t, &viewRay->dir);
hitpoint = vectorAdd(&viewRay->start, &scaled);
n=n1;
temp = vectorDot(&n,&n);
if (temp == 0.0f) break;
temp = invsqrtf(temp);
n = vectorScale(temp, &n);
currentMat = myScene->materials[myScene->triangles[currentTriangle].material];
} else {
/* No hit - add background */
colour test = {0.05,0.05,0.35};
colour tmp = colourCoefMul(coef, &test);
output = colourAdd(&output,&tmp);
break;
}
/* Bump mapping using Perlin noise */
if (currentMat.bump != 0.0) {
double noiseCoefx = noise(0.1 * hitpoint.x, 0.1 * hitpoint.y, 0.1 * hitpoint.z, myNoise);
double noiseCoefy = noise(0.1 * hitpoint.y, 0.1 * hitpoint.z, 0.1 * hitpoint.x, myNoise);
double noiseCoefz = noise(0.1 * hitpoint.z, 0.1 * hitpoint.x, 0.1 * hitpoint.y, myNoise);
n.x = (1.0f - currentMat.bump ) * n.x + currentMat.bump * noiseCoefx;
n.y = (1.0f - currentMat.bump ) * n.y + currentMat.bump * noiseCoefy;
n.z = (1.0f - currentMat.bump ) * n.z + currentMat.bump * noiseCoefz;
temp = vectorDot(&n, &n);
if (temp == 0.0f) {
break;
}
temp = invsqrtf(temp);
n = vectorScale(temp, &n);
}
bool inside;
if (vectorDot(&n,&viewRay->dir) > 0.0f) {
n = vectorScale(-1.0f,&n);
inside = TRUE;
} else {
inside = FALSE;
}
if (!inside) {
ray lightRay;
lightRay.start = hitpoint;
/* Find the value of the light at this point */
unsigned int j;
for (j = 0; j < myScene->numLights; ++j) {
light currentLight = myScene->lights[j];
lightRay.dir = vectorSub(¤tLight.pos,&hitpoint);
double lightprojection = vectorDot(&lightRay.dir,&n);
if ( lightprojection <= 0.0f ) continue;
double lightdist = vectorDot(&lightRay.dir,&lightRay.dir);
double temp = lightdist;
if ( temp == 0.0f ) continue;
temp = invsqrtf(temp);
lightRay.dir = vectorScale(temp,&lightRay.dir);
lightprojection = temp * lightprojection;
/* Calculate the shadows */
bool inshadow = FALSE;
double t = lightdist;
unsigned int k;
for (k = 0; k < myScene->numSpheres; ++k) {
if (collideRaySphere(&lightRay, &myScene->spheres[k], &t)) {
inshadow = TRUE;
break;
}
}
for (k = 0; k < myScene->numTriangles; ++k) {
if (collideRayTriangle(&lightRay, &myScene->triangles[k], &t, &n1)) {
inshadow = TRUE;
break;
}
}
if (!inshadow) {
/* Diffuse refraction using Lambert */
double lambert = vectorDot(&lightRay.dir, &n) * coef;
double noiseCoef = 0.0f;
int level = 0;
switch (currentMat.MatType) {
case TURBULENCE:
for (level = 1; level < 10; level++) {
noiseCoef += (1.0f / level )
* fabsf(noise(level * 0.05 * hitpoint.x,
level * 0.05 * hitpoint.y,
level * 0.05 * hitpoint.z,
myNoise));
}
colour diff1 = colourCoefMul(noiseCoef,¤tMat.diffuse);
colour diff2 = colourCoefMul((1.0f - noiseCoef), ¤tMat.mdiffuse);
colour temp1 = colourAdd(&diff1, &diff2);
output = colourAdd(&output, &temp1);
break;
case MARBLE:
for (level = 1; level < 10; level ++) {
noiseCoef += (1.0f / level)
* fabsf(noise(level * 0.05 * hitpoint.x,
level * 0.05 * hitpoint.y,
level * 0.05 * hitpoint.z,
myNoise));
}
noiseCoef = 0.5f * sinf( (hitpoint.x + hitpoint.y) * 0.05f + noiseCoef) + 0.5f;
colour diff3 = colourCoefMul(noiseCoef,¤tMat.diffuse);
colour diff4 = colourCoefMul((1.0f - noiseCoef), ¤tMat.mdiffuse);
colour tmp1 = colourAdd(&diff3, &diff4);
colour lamint = colourCoefMul(lambert, ¤tLight.intensity);
colour lamint_scaled = colourCoefMul(coef, &lamint);
colour temp2 = colourMul(&lamint_scaled, &tmp1);
output = colourAdd(&output, &temp2);
break;
/* Basically Gouraud */
default:
output.red += lambert * currentLight.intensity.red * currentMat.diffuse.red;
output.green += lambert * currentLight.intensity.green * currentMat.diffuse.green;
output.blue += lambert * currentLight.intensity.blue * currentMat.diffuse.blue;
break;
}
/* Blinn */
double viewprojection = vectorDot(&viewRay->dir, &n);
vector blinnDir = vectorSub(&lightRay.dir, &viewRay->dir);
double temp = vectorDot(&blinnDir, &blinnDir);
if (temp != 0.0f ) {
double blinn = invsqrtf(temp) * max(lightprojection - viewprojection , 0.0f);
blinn = coef * powf(blinn, currentMat.power);
colour tmp_1 = colourCoefMul(blinn, ¤tMat.specular);
colour tmp_2 = colourMul(&tmp_1, ¤tLight.intensity);
output = colourAdd(&output, &tmp_2);
}
}
}
/* Iterate over the reflection */
coef *= currentMat.reflection;
double reflect = 2.0f * vectorDot(&viewRay->dir, &n);
viewRay->start = hitpoint;
vector tmp = vectorScale(reflect, &n);
viewRay->dir = vectorSub(&viewRay->dir, &tmp);
}
level++;
/* Limit iteration depth to 10 */
} while ((coef > 0.0f) && (level < 10));
return output;
}
bool handleCollision(Contact *contact) {
Object *source = contact->object1;
Object *target = contact->object2;
float *normal = contact->normal;
float *contactpoint = contact->position;
float sourcevelocity[3], targetvelocity[3];
float sourcecontactpoint[3], targetcontactpoint[3];
vectorSub(sourcecontactpoint, contactpoint, source->position);
source->getVelocity(sourcevelocity, sourcecontactpoint);
if (target == NULL) {
vectorSet(targetcontactpoint, 0, 0, 0);
vectorSet(targetvelocity, 0, 0, 0);
} else {
vectorSub(targetcontactpoint, contactpoint, target->position);
target->getVelocity(targetvelocity, targetcontactpoint);
}
float deltavelocity[3];
vectorSub(deltavelocity, sourcevelocity, targetvelocity);
float dot = vectorDot(deltavelocity, normal);
// if (fabs(dot) < EPSILON) return false;
// if (dot > -1.0e-5 && dot < 1.0e-5) return false;
// if (dot >= 0) return false;
if (dot > -1.0e-5)
return false;
float invmass1;
invmass1 = source->invmass;
float invmass2;
if (target == NULL)
invmass2 = 0;
else
invmass2 = target->invmass;
float t1;
if (source->invmomentofinertia == 0) {
t1 = 0;
} else {
float v1[3];
vectorCross(v1, sourcecontactpoint, normal);
vectorScale(v1, source->invmomentofinertia);
float w1[3];
vectorCross(w1, v1, sourcecontactpoint);
t1 = vectorDot(normal, w1);
}
float t2;
if (target == NULL || target->invmomentofinertia == 0) {
t2 = 0;
} else {
float v1[3];
vectorCross(v1, targetcontactpoint, normal);
vectorScale(v1, target->invmomentofinertia);
float w1[3];
vectorCross(w1, v1, targetcontactpoint);
t2 = vectorDot(normal, w1);
}
float denominator = invmass1 + invmass2 + t1 + t2;
float e = 1.0 - COLLISIONFRICTION;
float impulsesize = (1 + e) * dot / denominator;
// printf("%f\n", impulsesize);
float impulse[3];
vectorScale(impulse, normal, impulsesize);
float friction[3];
vectorScale(friction, normal, vectorDot(deltavelocity, normal));
vectorAdd(friction, deltavelocity);
vectorNormalize(friction);
float frictionsize = 10 * KINETICFRICTION * dot / denominator;
float maxfrictionsize = 0.1 * vectorLength(deltavelocity);
if (frictionsize < -maxfrictionsize)
frictionsize = -maxfrictionsize;
vectorScale(friction, -frictionsize);
vectorAdd(impulse, friction);
if (target != NULL) {
target->addImpulse(impulse, targetcontactpoint);
target->calculateStateVariables();
}
float speed;
float speed2[3];
if (target != NULL && source != NULL) {
// float kvel[3];
// source->getVelocity(kvel);
float k = vectorLength(sourcevelocity) * 0.1;
// if (k > 1) k = 1;
speed = -impulsesize * target->invmass * k;
vectorScale(speed2, impulse, target->invmass * k);
/*float kvel[3];
source->getVelocity(kvel);
float k = 0;//vectorDot(speed2, kvel);
if (k < EPSILON) k = 0;
speed *= k;
vectorScale(speed2, k);
if (k > 0) */ target->hitForce(speed, speed2, source);
}
vectorScale(impulse, -1);
source->addImpulse(impulse, sourcecontactpoint);
source->calculateStateVariables();
// vectorScale(speed, source->invmass);
if (target != NULL && source != NULL) {
// float kvel[3];
// target->getVelocity(kvel);
float k = vectorLength(targetvelocity) * 0.1;
// if (k > 1) k = 1;
speed = -impulsesize * source->invmass * k;
vectorScale(speed2, impulse, source->invmass * k);
/*float kvel[3];
target->getVelocity(kvel);
float k = 0;//vectorDot(speed2, kvel);
if (k < EPSILON) k = 0;
speed *= k;
vectorScale(speed2, k);
if (k > 0) */ source->hitForce(speed, speed2, target);
}
return true;
}
bool checkSphereMeshCollision(float *sphereposition, float r, Mesh *mesh,
float *normal, float *contactpoint) {
float linenormal[3];
float pointnormal[3];
float maxdist = 0;
bool planecollision = false;
bool linecollision = false;
bool pointcollision = false;
int i, j;
for (i = 0; i < mesh->polygoncount; i++) {
class Polygon *polygon = &mesh->polygons[i];
float dist = distanceFromPlane(sphereposition, polygon->planenormal,
polygon->planedistance);
if (dist < r && dist > -r) {
bool directcollision = true;
for (j = 0; j < polygon->vertexcount; j++) {
float *p1 = polygon->vertices[j]->position;
float *p2 = polygon->vertices[(j + 1) % polygon->vertexcount]->position;
float *p3 = polygon->vertices[(j + 2) % polygon->vertexcount]->position;
float v1[3], v2[3];
vectorSub(v1, p2, p1);
// Collision for polygon surface
vectorSub(v2, p3, p2);
float t1[3];
vectorProject(t1, v2, v1);
float norm[3];
vectorSub(norm, v2, t1);
vectorNormalize(norm);
// Collision for polygon edges
float newpoint[3];
vectorSub(newpoint, sphereposition, p1);
float dist2 = vectorDot(newpoint, norm);
if (dist2 < 0) {
directcollision = false;
float projloc = vectorDot(newpoint, v1) / vectorDot(v1, v1);
if (projloc >= 0 && projloc <= 1) {
float proj[3];
vectorScale(proj, v1, projloc);
float projorth[3];
vectorSub(projorth, newpoint, proj);
float l2 = vectorDot(projorth, projorth);
if (l2 < r * r) {
vectorNormalize(linenormal, projorth);
if (dist < 0)
vectorScale(linenormal, -1);
linecollision = true;
}
}
}
// Collision for polygon vertices
float pointdiff[3];
vectorSub(pointdiff, sphereposition, p1);
float l3 = vectorDot(pointdiff, pointdiff);
if (l3 < r * r) {
vectorScale(pointnormal, pointdiff, 1.0 / sqrt(l3));
if (dist < 0)
vectorScale(pointnormal, -1);
pointcollision = true;
}
}
if (directcollision) {
if (dist > maxdist || !planecollision) {
vectorCopy(normal, polygon->planenormal);
maxdist = dist;
planecollision = true;
}
}
}
}
if (planecollision) {
vectorScale(contactpoint, normal, -r);
vectorAdd(contactpoint, sphereposition);
} else if (linecollision) {
vectorScale(contactpoint, linenormal, -r);
vectorAdd(contactpoint, sphereposition);
vectorCopy(normal, linenormal);
} else if (pointcollision) {
vectorScale(contactpoint, pointnormal, -r);
vectorAdd(contactpoint, sphereposition);
vectorCopy(normal, pointnormal);
} else {
return false;
}
return true;
}
bool handleLink(ObjectLink *link) {
if (!link->enabled)
return false;
Object *source = link->object1;
Object *target = link->object2;
float normal[3];
float contactpoint1[3], contactpoint2[3];
source->transformPoint(contactpoint1, link->point1);
target->transformPoint(contactpoint2, link->point2);
float diff[3];
vectorSub(diff, contactpoint1, contactpoint2);
vectorNormalize(normal, diff);
float strength = vectorDot(diff, diff);
if (strength < 1.0e-5)
return false;
float sourcevelocity[3], targetvelocity[3];
float sourcecontactpoint[3], targetcontactpoint[3];
vectorSub(sourcecontactpoint, contactpoint1, source->position);
source->getVelocity(sourcevelocity, sourcecontactpoint);
vectorSub(targetcontactpoint, contactpoint2, target->position);
target->getVelocity(targetvelocity, targetcontactpoint);
float deltavelocity[3];
vectorSub(deltavelocity, sourcevelocity, targetvelocity);
float dot = vectorDot(deltavelocity, normal);
// if (fabs(dot) < EPSILON) return false;
// if (dot > -1.0e-5 && dot < 1.0e-5) return false;
// if (dot >= 0) return false;
// if (dot > -1.0e-5) return false;
float invmass1 = source->invmass;
float invmass2 = target->invmass;
float t1;
if (source->invmomentofinertia == 0) {
t1 = 0;
} else {
float v1[3];
vectorCross(v1, sourcecontactpoint, normal);
vectorScale(v1, source->invmomentofinertia);
float w1[3];
vectorCross(w1, v1, sourcecontactpoint);
t1 = vectorDot(normal, w1);
}
float t2;
if (target->invmomentofinertia == 0) {
t2 = 0;
} else {
float v1[3];
vectorCross(v1, targetcontactpoint, normal);
vectorScale(v1, target->invmomentofinertia);
float w1[3];
vectorCross(w1, v1, targetcontactpoint);
t2 = vectorDot(normal, w1);
}
float denominator = invmass1 + invmass2 + t1 + t2;
float impulsesize = (dot + strength * 100) / denominator;
// printf("%f\n", impulsesize);
float impulse[3];
vectorScale(impulse, normal, impulsesize);
target->addImpulse(impulse, targetcontactpoint);
target->calculateStateVariables();
vectorScale(impulse, -1);
source->addImpulse(impulse, sourcecontactpoint);
source->calculateStateVariables();
return true;
}
