//-----------------------------------------------
bool CPUTFrustum::IsVisible(
const float3 ¢er,
const float3 &half
){
UINT ii;
float3 absHalf = abs3(half);
float3 planeToPoint = center - mpPosition[0]; // Use near-clip-top-left point for point on first three planes
for( ii=0; ii<3; ii++ )
{
float3 normal = mpNormal[ii];
float3 absNormal = abs3(normal);
float nDotC = dot3( normal, planeToPoint );
if( nDotC > dot3( abs3(normal), absHalf ) )
{
return false;
}
}
planeToPoint = center - mpPosition[6]; // Use near-clip-top-left point for point on first three planes
for( ii=3; ii<6; ii++ )
{
float3 normal = mpNormal[ii];
float3 absNormal = abs3(normal);
float nDotC = dot3( normal, planeToPoint );
if( nDotC > dot3( abs3(normal), absHalf ) )
{
return false;
}
}
// Tested all eight points against all six planes and none of the planes
// had all eight points outside.
return true;
}
int raySphere(float *r, float *s, float radius) {
float A = dot3(r,r);
float B = -2.0 * dot3(s,r);
float C = dot3(s,s) - radius;
float D = B*B - 4*A*C;
if (D > 0)
return 0;
}
bool intersect(Point p0, Point p1, Point q0, Point q1) {
PType c0 = cross3(p0, p1, q0), c1 = cross3(p0, p1, q1);
PType d0 = cross3(q0, q1, p0), d1 = cross3(q0, q1, p1);
if(c0 == 0 && c1 == 0) {
PType e0 = dot3(p0, p1, q0);
PType e1 = dot3(p0, p1, q1);
if( !(e0 < e1) ) {
swap(e0, e1);
}
return e0 <= dist2(p0, p1) && 0 <= e1;
}
return (c0 ^ c1) <= 0 && (d0 ^ d1) <= 0;
}
CPUTCamera &CPUTCamera::operator=(const CPUTCamera& camera)
{
mFov = camera.mFov;
mNearPlaneDistance = camera.mNearPlaneDistance;
mFarPlaneDistance = camera.mFarPlaneDistance;
mAspectRatio = camera.mAspectRatio;
mView = camera.mView;
mProjection = camera.mProjection;
for(int i = 0; i < 8; i++)
{
mFrustum.mpPosition[i] = camera.mFrustum.mpPosition[i];
}
for(int i = 0; i < 6; i++)
{
mFrustum.mpNormal[i] = camera.mFrustum.mpNormal[i];
mFrustum.mPlanes[0*8 + i] = mFrustum.mpNormal[i].x;
mFrustum.mPlanes[1*8 + i] = mFrustum.mpNormal[i].y;
mFrustum.mPlanes[2*8 + i] = mFrustum.mpNormal[i].z;
mFrustum.mPlanes[3*8 + i] = -dot3(mFrustum.mpNormal[i], mFrustum.mpPosition[(i < 3) ? 0 : 6]);
}
for (int i=6; i < 8; i++)
{
mFrustum.mPlanes[0*8 + i] = 0;
mFrustum.mPlanes[1*8 + i] = 0;
mFrustum.mPlanes[2*8 + i] = 0;
mFrustum.mPlanes[3*8 + i] = -1.0f;
}
return *this;
}
float noise ( float x, float y, float z )
{
int X = floor(x);
int Y = floor(y);
int Z = floor(z);
// Get relative xyz coordinates of point within that cell
x = x - X;
y = y - Y;
z = z - Z;
// Wrap the integer cells at 255 (smaller integer period can be introduced here)
X = X & 255;
Y = Y & 255;
Z = Z & 255;
// Calculate a set of eight hashed gradient indices
int gi000 = perm[X + perm[Y + perm[Z]]] % 12;
int gi001 = perm[X + perm[Y + perm[Z + 1]]] % 12;
int gi010 = perm[X + perm[Y + 1 + perm[Z]]] % 12;
int gi011 = perm[X + perm[Y + 1 + perm[Z + 1]]] % 12;
int gi100 = perm[X + 1 + perm[Y + perm[Z]]] % 12;
int gi101 = perm[X + 1 + perm[Y + perm[Z + 1]]] % 12;
int gi110 = perm[X + 1 + perm[Y + 1 + perm[Z]]] % 12;
int gi111 = perm[X + 1 + perm[Y + 1 + perm[Z + 1]]] % 12;
// The gradients of each corner are now:
// g000 = grad3[gi000];
// g001 = grad3[gi001];
// g010 = grad3[gi010];
// g011 = grad3[gi011];
// g100 = grad3[gi100];
// g101 = grad3[gi101];
// g110 = grad3[gi110];
// g111 = grad3[gi111];
// Calculate noise contributions from each of the eight corners
float n000 = dot3( grad3[gi000], x, y, z );
float n100 = dot3( grad3[gi100], x - 1, y, z );
float n010 = dot3( grad3[gi010], x, y - 1, z );
float n110 = dot3( grad3[gi110], x - 1, y - 1, z );
float n001 = dot3( grad3[gi001], x, y, z - 1 );
float n101 = dot3( grad3[gi101], x - 1, y, z - 1 );
float n011 = dot3( grad3[gi011], x, y - 1, z - 1 );
float n111 = dot3( grad3[gi111], x - 1, y - 1, z - 1 );
// Compute the fade curve value for each of x, y, z
float u = fade( x );
float v = fade( y );
float w = fade( z );
// Interpolate along x the contributions from each of the corners
float nx00 = mix( n000, n100, u );
float nx01 = mix( n001, n101, u );
float nx10 = mix( n010, n110, u );
float nx11 = mix( n011, n111, u );
// Interpolate the four results along y
float nxy0 = mix( nx00, nx10, v );
float nxy1 = mix( nx01, nx11, v );
// Interpolate the two last results along z
float nxyz = mix( nxy0, nxy1, w );
return nxyz;
}
/**
* Identify bends in the chain, where the kappa angle (virtual bond angle from
* c-alpha i-2, to i, to i+2) is greater than 70 degrees
* dssp-2.2.0/structure.cpp:1729
*/
static std::vector<int> calculate_bends(const float* xyz, const int* ca_indices,
const int* chain_ids, const int n_residues, std::vector<int>& skip)
{
std::vector<int> is_bend(n_residues, 0);
for (int i = 2; i < n_residues-2; i++) {
if (chain_ids[i-2] == chain_ids[i+2] && !skip[i-2] && !skip[i] && !skip[i+2]) {
fvec4 prev_ca(xyz[3*ca_indices[i-2]], xyz[3*ca_indices[i-2]+1], xyz[3*ca_indices[i-2]+2], 0);
fvec4 this_ca(xyz[3*ca_indices[i]], xyz[3*ca_indices[i]+1], xyz[3*ca_indices[i]+2], 0);
fvec4 next_ca(xyz[3*ca_indices[i+2]], xyz[3*ca_indices[i+2]+1], xyz[3*ca_indices[i+2]+2], 0);
fvec4 u_prime = prev_ca-this_ca;
fvec4 v_prime = this_ca-next_ca;
float cosangle = dot3(u_prime, v_prime)/sqrtf(dot3(u_prime, u_prime)*dot3(v_prime, v_prime));
float kappa = acosf(CLIP(cosangle, -1, 1));
is_bend[i] = kappa > (70 * (M_PI / 180.0));
}
}
return is_bend;
}
void SE3_rectify(SE3 &s)
{
normalize3(&s.R[0]);
double ab = dot3(&s.R[0], &s.R[3]);
for (int i=0; i<3; ++i)
s.R[3+i] -= ab * s.R[i];
normalize3(&s.R[3]);
cross_product(&s.R[6], &s.R[0], &s.R[3]);
}
xyzVector* reflection(const xyzVector spherePoint, const xyzVector view) {
xyzVector* result = (xyzVector* ) malloc(sizeof(xyzVector));
float ndotv;
ndotv = dot3( spherePoint, view);
ndotv *= 2.0;
scale( spherePoint, ndotv, result );
sub( *result, view, result );
return result;
}
void
MeshElementTable::calcNormalDistance(int index)
{
if ( normalDistances == NULL ||
centers == NULL ||
normals == NULL
)
return;
normalDistances[index] = dot3(normals[index], centers[index]);
}
//-----------------------------------------------
bool CPUTFrustum::IsVisible(
const float3 ¢er,
const float3 &half
){
// TODO: There are MUCH more efficient ways to do this.
float3 pBoundingBoxPosition[8];
pBoundingBoxPosition[0] = center + float3( half.x, half.y, half.z );
pBoundingBoxPosition[1] = center + float3( half.x, half.y, -half.z );
pBoundingBoxPosition[2] = center + float3( half.x, -half.y, half.z );
pBoundingBoxPosition[3] = center + float3( half.x, -half.y, -half.z );
pBoundingBoxPosition[4] = center + float3( -half.x, half.y, half.z );
pBoundingBoxPosition[5] = center + float3( -half.x, half.y, -half.z );
pBoundingBoxPosition[6] = center + float3( -half.x, -half.y, half.z );
pBoundingBoxPosition[7] = center + float3( -half.x, -half.y, -half.z );
// Test each bounding box point against each of the six frustum planes.
// Note: we need a point on the plane to compute the distance to the plane.
// We only need two of our frustum's points to do this. A corner vertex is on
// three of the six planes. We need two of these corners to have a point
// on all six planes.
UINT pPointIndex[6] = {0,0,0,6,6,6};
UINT ii;
for( ii=0; ii<6; ii++ )
{
bool allEightPointsOutsidePlane = true;
float3 *pNormal = &mpNormal[ii];
float3 *pPlanePoint = &mpPosition[pPointIndex[ii]];
float3 planeToPoint;
float distanceToPlane;
UINT jj;
for( jj=0; jj<8; jj++ )
{
planeToPoint = pBoundingBoxPosition[jj] - *pPlanePoint;
distanceToPlane = dot3( *pNormal, planeToPoint );
if( distanceToPlane < 0.0f )
{
allEightPointsOutsidePlane = false;
break; // from for. No point testing any more points against this plane.
}
}
if( allEightPointsOutsidePlane )
{
mNumFrustumCulledModels++;
return false;
}
}
// Tested all eight points against all six planes and none of the planes
// had all eight points outside.
mNumFrustumVisibleModels++;
return true;
}
double ang_between(double x1[3], double x2[3]) {
double mydot = dot3(x1, x2);
double mag1 = mag3(x1);
double mag2 = mag3(x2);
double cos_ang = mydot/mag1/mag2;
if (cos_ang > 1)
cos_ang = 1;
if (cos_ang < -1)
cos_ang = -1;
return acos(cos_ang);
}
t_vector3f rotate3(t_vector3f vec, t_vector3f axis, float angle)
{
float sin_angle;
float cos_angle;
t_vector3f tmp;
sin_angle = (float)sin(-angle);
cos_angle = (float)cos(-angle);
tmp = cross3(vec, mul3f(axis, sin_angle));
tmp = add3v(tmp, mul3f(vec, cos_angle));
tmp = add3v(tmp, mul3f(vec, dot3(vec, mul3f(axis, 1 - cos_angle))));
return (tmp);
}
static int luaB_dot (lua_State *L) {
float x1, y1, z1, w1;
float x2, y2, z2, w2;
if (lua_gettop(L) != 2) luaL_error(L, "Invalid params, try dot(v,v)");
if (lua_isvector4(L,1)) {
lua_checkvector4(L, 1, &x1, &y1, &z1, &w1);
lua_checkvector4(L, 2, &x2, &y2, &z2, &w2);
lua_pushnumber(L,dot4(x1,y1,z1,w1, x2,y2,z2,w2));
} else if (lua_isvector3(L,1)) {
lua_checkvector3(L, 1, &x1, &y1, &z1);
lua_checkvector3(L, 2, &x2, &y2, &z2);
lua_pushnumber(L,dot3(x1,y1,z1, x2,y2,z2));
} else if (lua_isvector2(L,1)) {
lua_checkvector2(L, 1, &x1, &y1);
lua_checkvector2(L, 2, &x2, &y2);
lua_pushnumber(L,dot2(x1,y1, x2,y2));
}
return 1;
}
void
MeshElementTable::calcCenter(int index)
{
if (centers == NULL)
return;
const int* nodeIds = getNodeIds(index);
// Create center point
Point3& center = centers[index];
centerPoint(index, center);
// Calculate square of the distance of the first node
// from the center
Point3 r;
diff3(meshNodes[nodeIds[0]], center, r);
double rsquared = dot3(r, r);
rSquares[index] = rsquared;
}
//<<<<<<<<<<<<<<<<<<<<<< checkOrthogVects >>>>>>>>>>>>>>>>
void checkOrthogVects(Vector3 a, Vector3 b, Vector3 c)
{ // check that system is orthonormal
double aDotb = fabs(dot3(a, b));
double aDotc = fabs(dot3(a, c));
double bDotc = fabs(dot3(b, c));
if( aDotb > 0.000001 || aDotc > 0.000001 || bDotc > 0.000001)
cout << " Bad!! vectors NOT orthogonal!\n";
double A = fabs(dot3(a,a) - 1.0);
double B = fabs(dot3(b,b) - 1.0);
double C = fabs(dot3(c,c) - 1.0);
#if 0
if(A > 0.0000001)
cout << "Bad!! first is not normalized!\n";
if( B > 0.0000001)
cout << "Bad!! second is not normalized!\n";
if( C > 0.0000001)
cout << "Bad!! third is not normalized!\n";
#endif
}
double mag3(double x[3]) {
double y = dot3(x, x);
return sqrt(y);
}
static int luaB_quat (lua_State *L) {
if (lua_gettop(L)==4 && lua_isnumber(L,1) && lua_isnumber(L,2) && lua_isnumber(L,3) && lua_isnumber(L,4)) {
float w,x,y,z;
w = (float)lua_tonumber(L, 1);
x = (float)lua_tonumber(L, 2);
y = (float)lua_tonumber(L, 3);
z = (float)lua_tonumber(L, 4);
lua_pushquat(L, w,x,y,z);
return 1;
} else if (lua_gettop(L)==2 && lua_isnumber(L,1) && lua_isvector3(L,2)) {
float angle,x,y,z,ha,s;
angle = (float)lua_tonumber(L, 1);
lua_checkvector3(L, 2, &x, &y, &z);
ha = angle*(0.5f*PI/180.0f);
s = sinf(ha);
lua_pushquat(L, cosf(ha),s*x,s*y,s*z);
return 1;
} else if (lua_gettop(L)==2 && lua_isvector3(L,1) && lua_isvector3(L,2)) {
float x1, y1, z1;
float x2, y2, z2;
float l1,l2,d;
lua_checkvector3(L, 1, &x1, &y1, &z1);
lua_checkvector3(L, 2, &x2, &y2, &z2);
/* Based on Stan Melax's article in Game Programming Gems */
l1 = sqrtf(x1*x1 + y1*y1 + z1*z1);
l2 = sqrtf(x2*x2 + y2*y2 + z2*z2);
x1/=l1; y1/=l1; z1/=l1;
x2/=l2; y2/=l2; z2/=l2;
d = dot3(x1,y1,z1, x2,y2,z2);
/* If dot == 1, vectors are the same */
if (d >= 1.0f) {
lua_pushquat(L, 1,0,0,0);
return 1;
}
if (d < (1e-6f - 1.0f)) {
float ax, ay, az;
float len2, len;
cross3(1,0,0, x1,y1,z1, &ax, &ay, &az);
len2 = ax*ax + ay*ay + az*az;
if (len2 == 0) {
cross3(0,1,0, x1,y1,z1, &ax, &ay, &az);
len2 = ax*ax + ay*ay + az*az;
}
len = sqrtf(len2);
ax/=len; ay/=len; az/=len;
lua_pushquat(L, 0,ax,ay,az);
return 1;
} else {
float s = sqrtf((1+d)*2);
float ax, ay, az;
float qw, qlen;
cross3(x1,y1,z1, x2,y2,z2, &ax, &ay, &az);
ax/=s; ay/=s; az/=s;
qw = s*0.5f;
qlen = sqrtf(qw*qw + ax*ax + ay*ay + az*az);
lua_pushquat(L, qw/qlen,ax/qlen,ay/qlen,az/qlen);
return 1;
}
} else {
luaL_error(L, "Invalid params, try quat(n,n,n,n) quat(n,v3) quat(v3,v3)");
return 0;
}
}
Real Vector4::dotabs3(const Vector3& other) const
{
return Math::Abs(dot3(other));
}
float length_sq(const float3 &vec)
{
return dot3(vec, vec);
}
Real Vector4::dot3(const Vector4& other) const
{
return dot3(other.toVector3());
}
void timestep(graphics_state *state){
int h = state->frame;
/* eye, moves with time */
*((f3*)state->projection_matrix + 3) = (f3) {
0, //sinf(h / 30.0f),
0,
-4}; //-cosf(h / 30.0f)};
/* so does the light */
int i;
for(i = 0; i < state->light_count; i++){
light *light = state->lights + i;
light->location = (f4){
sinf(h / 50.0f + i)*cosf(h / 100.0f + i),
sinf(h/100.0f + i),
cosf(h / 50.0f + i)*cosf(h / 100.0f + i),
0.2f};
}
/* so do the objs */
for(i = 0; i < state->object_count; i++){
sphere *s = state->object_list + i;
s->center.x += s->velocity.x;
s->center.y += s->velocity.y;
s->center.z += s->velocity.z;
/* bounce off the bounding box */
if(s->center.x < state->bounds.top.x)
s->velocity.x = absf(s->velocity.x);
if(s->center.y < state->bounds.top.y)
s->velocity.y = absf(s->velocity.y);
if(s->center.z < state->bounds.top.z)
s->velocity.z = absf(s->velocity.z);
if(s->center.x > state->bounds.bottom.x)
s->velocity.x = -absf(s->velocity.x);
if(s->center.y > state->bounds.bottom.y)
s->velocity.y = -absf(s->velocity.y);
if(s->center.z > state->bounds.bottom.z)
s->velocity.z = -absf(s->velocity.z);
/* bounce off each other */
int j;
for(j = i+1; j < state->object_count; j++){
sphere *s2 = state->object_list + j;
f3 diff = (f3){
s->center.x - s2->center.x,
s->center.y - s2->center.y,
s->center.z - s2->center.z};
float dist = norm3(&diff);
if(dist < s->radius + s2->radius){
normalize3(&diff);
float dot = dot3(&diff, &s->velocity);
if(dot > 0)
/* if they're already flying apart,
* don't bounce them back towards each other */
continue;
float dot2 = dot3(&diff, &s2->velocity);
s->velocity.x -= diff.x*(dot - dot2);
s->velocity.y -= diff.y*(dot - dot2);
s->velocity.z -= diff.z*(dot - dot2);
s2->velocity.x -= diff.x*(dot2 - dot);
s2->velocity.y -= diff.y*(dot2 - dot);
s2->velocity.z -= diff.z*(dot2 - dot);
}
}
}
}
//-----------------------------------------------------------------------------
void ProjectVerticesOntoTriangles(
float3 *pSrcPositions, // Positions of vertices to project
UINT srcVertexCount, // Number of vertices to project
UINT triangleCount, // Number of triangles in mesh projecting onto
UINT *pTriangleIndices, // Vertex indices for each triangle in mesh being projected onto
float3 *pTriangleVertexPositions, // Position of each vertex in mesh being projected onto
float3 *pTriangleVertexNormals, // Normal of each vertex in mesh being projected onto
MappedVertex *pMapping, // Output mapping results
float frontFacingDistanceThreshold, // Don't map vertices to front-facing triangles further than this distance away
float backFacingDistanceThreshold // Don't map vertices to back-facing triangles further than this distance away
){
// Determine which triangle that each vertex projects onto
for (UINT vIdx = 0; vIdx < srcVertexCount; vIdx++)
{
pMapping[vIdx].ClosestDistance = FLT_MAX; // Want to save smallest, so init to largest possible
float shortestLength = FLT_MAX;
float3 vv = pSrcPositions[vIdx];
// Initialize the barycentrics to known bad value so we can later verify they're set to good value
pMapping[vIdx].BarycentricCoordinates = float3(-1.0f);
// Find the closest triangle this vertex projects onto (note: actually project the triangles onto the vertex)
for (UINT tIdx = 0; tIdx < triangleCount; tIdx++)
{
// triangle's vertex indices
UINT i0 = pTriangleIndices[tIdx * 3 + 0];
UINT i1 = pTriangleIndices[tIdx * 3 + 1];
UINT i2 = pTriangleIndices[tIdx * 3 + 2];
// triangle's vertex positions
float3 v0 = pTriangleVertexPositions[i0];
float3 v1 = pTriangleVertexPositions[i1];
float3 v2 = pTriangleVertexPositions[i2];
// triangle's normal
float3 normal = cross3(v1 - v0, v2 - v0);
float rcpArea = 1.0f / normal.length();
normal *= rcpArea;
// Vector from triangle (vertex 0) to vertex
float3 triangleToVertex = vv - v0;
float dd = dot3(triangleToVertex, normal); // Closest distance from vertex to plane containing the triangle
// TODO: Assuming vertex normal follows position. Assert, etc
float3 n0 = pTriangleVertexNormals[i0];
float3 n1 = pTriangleVertexNormals[i1];
float3 n2 = pTriangleVertexNormals[i2];
// offsetT# is vertex# offset along it's normal so resulting triangle plane contains the vertex
float3 offsetT0 = v0 + n0 * dd / dot3(n0, normal);
float3 offsetT1 = v1 + n1 * dd / dot3(n1, normal);
float3 offsetT2 = v2 + n2 * dd / dot3(n2, normal);
// Compute area for projected triangle (well, 2X area. Later divide removes factor of 2)
float3 crossOffset = cross3(offsetT1 - offsetT0, offsetT2 - offsetT0);
float area = crossOffset.length();
float rcpOffsetArea = 1.0f / area;
// Start computing barycentric coordinates. Each is 2X area of tri formed by one projected triangle edge and the vertex
float3 aa = cross3(offsetT2 - offsetT1, vv - offsetT1);
float3 bb = cross3(offsetT0 - offsetT2, vv - offsetT2);
float3 cc = cross3(offsetT1 - offsetT0, vv - offsetT0);
// Compute distance to triangle so we can reject intersections with distant triangles.
// This addresses the issue that hair can extend beyond the mesh (e.g., below the neck)
float3 barys = float3( aa.length(), bb.length(), cc.length() ) * rcpOffsetArea;
float barySum = barys.x + barys.y + barys.z;
if( barySum <= 1.00001f ) // Note slightly > 1.0 to accomodate floating point errors.
{
float3 intersection = barys.x*v0 + barys.y*v1 + barys.z*v2;
float3 ray = vv - intersection;
float length = ray.length();
bool frontFacing = dot3(ray, normal) > 0.0f;
if( ( ( frontFacing && length < frontFacingDistanceThreshold )
|| ( !frontFacing && length < backFacingDistanceThreshold ) )
&& (length < shortestLength) )
{
shortestLength = length;
pMapping[vIdx].TriangleIndex = tIdx; // vertex[vIdx] projects onto triangle[hIdx]
pMapping[vIdx].ClosestDistance = length; // and is dd distance away (along the triangle normal)
pMapping[vIdx].BarycentricCoordinates = barys; // and intersects at the point with these barycentric coordinates
}
}
}
}
}
// Physics thread that controls the motion of all the spheres.
void physicsThread(int threadNum)
{
int localFrame = 0;
// Define the normals for each of the walls.
static const Vector3 bottom(0, 1, 0);
static const Vector3 left(1, 0, 0);
static const Vector3 right(-1, 0, 0);
static const Vector3 front(0, 0, -1);
static const Vector3 back(0, 0, 1);
// Initialize the starting values for the wall and sphere, spring and damping values.
float wallSpringConst = UPPER_WALL_SPRING_CONST;
float sphereSpringConst = UPPER_SPHERE_SPRING_CONST;
float wallDampFactor = UPPER_WALL_DAMP_FACTOR;
float sphereDampFactor = UPPER_SPHERE_DAMP_FACTOR;
// The physics thread itself deals with reseting its frame counter.
// It will do this once a second.
float secondCheck = 0.0f;
#if _USE_MT
// If the physics function is being called as a thread, then we don't want it
// to finish after one pass.
int threadRunning = 0;
while(programRunning)
{
// If the simulation needs to be paused but the thread is running,
// stop.
if(pauseSimulation && threadRunning)
{
threadRunning = 0;
threadStopped();
}
// If the simulation needs to be running but the thread is stopped,
// start.
else if(!pauseSimulation && !threadRunning)
{
threadRunning = 1;
threadStarted();
}
// If the thread isn't running at this time we can't go any further and
// we'll just wait and check if the thread has started up next time the
// thread is executing.
if(!threadRunning)
{
boost::this_thread::yield();
continue;
}
#endif
localFrame++;
LARGE_INTEGER time;
QueryPerformanceCounter(&time);
// Get the time since this thread last executed and convert into
// seconds (from milliseconds).
float dt = (float)(((double)time.QuadPart - (double)updateTimes[threadNum].QuadPart) / freq);
dt *= 0.001;
// Adjust the spring and dampening values based on dt.
// This helps when going to lower frame rates and the overlap between
// the walls and spheres when using high frame rate values will cause
// the spheres to gain energy.
wallSpringConst = WALL_SPRING_GRAD * dt + WALL_SPRING_CONST;
sphereSpringConst = SPHERE_SPRING_GRAD * dt + SPHERE_SPRING_CONST;
wallDampFactor = WALL_DAMP_GRAD * dt + WALL_DAMP_CONST;
sphereDampFactor = SPHERE_DAMP_GRAD * dt + SPHERE_DAMP_CONST;
// As the gradients for the spring and dampening factors are negative,
// we want to clamp the lower bounds of the values. Otherwise at low
// framerates the values will be negative.
if (wallSpringConst < LOWER_WALL_SPRING_CONST)
wallSpringConst = LOWER_WALL_SPRING_CONST;
if (sphereSpringConst < LOWER_SPHERE_SPRING_CONST)
sphereSpringConst = LOWER_SPHERE_SPRING_CONST;
if (wallDampFactor < LOWER_WALL_DAMP_FACTOR)
wallDampFactor = LOWER_WALL_DAMP_FACTOR;
if (sphereDampFactor < LOWER_SPHERE_DAMP_FACTOR)
sphereDampFactor = LOWER_SPHERE_DAMP_FACTOR;
// If this is the 1st thread, then we will deal with the user controlled
// sphere here.
int startSphere = threadNum;
if(threadNum == 0)
{
// Skip calcuating the user sphere like a typical sphere.
startSphere += numPhysicsThreads;
// As the user controlled sphere only has a position and velocity
// that is either zero or constant, we can easily calculate its new
// position.
if(numSpheres > 0)
{
spherePositions[0] += sphereData[0].m_velocity * dt;
}
}
for(int i = startSphere; i < numSpheres; i += numPhysicsThreads)
{
// Calculate the radius of the sphere based on array index.
float radius = (float)((i % 3) + 1) * 0.5f;
float roomSize = ROOM_SIZE - radius;
Vector3 forces(0);
Vector3 &spherePosition = spherePositions[i];
SphereData &sphere = sphereData[i];
// Calculate the interim velocity.
Vector3 halfVelo = sphere.m_velocity + (0.5f * sphere.m_acc * dt);
spherePosition += halfVelo * dt;
Vector3 tempVelocity = sphere.m_velocity + (sphere.m_acc * dt);
float overlap;
// As the % operator is fairly slow we can take advantage here
// that we always know what the next value in the array is going
// to be and manually determine the mod.
int indexCounter = 0;
for(int j = 0; j < numSpheres; j++)
{
// And since we don't want the actual mod value but mod + 1
// we don't worry about resetting to 0.
indexCounter++;
if(indexCounter > 3)
indexCounter = 1;
// We don't want a sphere to check if it's collided with itself.
if(i == j)
continue;
SphereData &otherSphere = sphereData[j];
Vector3 toCentre = spherePosition - spherePositions[j];
// An unfortunately slow way of checking if the radius of the
// other sphere should be the other spheres actual radius or
// the user controlled spheres radius.
float otherRadius;
if(j == 0)
{
otherRadius = userSphereSize;
}
else
{
otherRadius = (float)(indexCounter) * 0.5f;
}
float combinedRadius = radius + otherRadius;
float lengthSqrd = lengthSquared(toCentre);
float combinedRadiusSqrd = combinedRadius * combinedRadius;
// A check to see if the spheres have overlapped at all.
float overlapSqrd = lengthSqrd - combinedRadiusSqrd;
if(overlapSqrd < 0)
{
#if _SSE
overlap = sqrt(lengthSqrd) - combinedRadius;
// We want to let the vector normalize itself in SSE
// because we can take advantage of the rsqrt instruction
// which will be quicker than loading in a float value
// and multiplying by the reciprocal.
Vector3 tangent = normalize(toCentre);
#else
// Here we can take advantage that we've already calculated
// the actual length value so that we have the overlap and
// can use that value again to normalize the tangent.
float len = sqrt(lengthSqrd);
overlap = len - combinedRadius;
Vector3 tangent = toCentre / len;
#endif
calcForce(forces, sphereSpringConst, sphereDampFactor, overlap, tangent, tempVelocity);
}
}
// Calculate the overlap with the walls using the normal of the wall
// and the walls distance from the origin. Should work best for SSE
// as it should not require any checking of individual elements of
// the vector.
overlap = dot3(spherePosition, bottom) + roomSize;
if(overlap < 0)
{
calcForce(forces, wallSpringConst, wallDampFactor, overlap, bottom, tempVelocity);
}
overlap = dot3(spherePosition, left) + roomSize;
if(overlap < 0)
{
calcForce(forces, wallSpringConst, wallDampFactor, overlap, left, tempVelocity);
}
overlap = dot3(spherePosition, right) + roomSize;
if(overlap < 0)
{
calcForce(forces, wallSpringConst, wallDampFactor, overlap, right, tempVelocity);
}
overlap = dot3(spherePosition, front) + roomSize;
if(overlap < 0)
{
calcForce(forces, wallSpringConst, wallDampFactor, overlap, front, tempVelocity);
}
overlap = dot3(spherePosition, back) + roomSize;
if(overlap < 0)
{
calcForce(forces, wallSpringConst, wallDampFactor, overlap, back, tempVelocity);
}
// Apply the accumulated forces to the acceleration.
sphere.m_acc = forces / (radius * 2.0f);
sphere.m_acc += GRAVITY;
// Calculate the final velocity value from the interim velocity
// and final acceleration.
sphere.m_velocity = halfVelo + (0.5f * sphere.m_acc * dt);
}
// Determin if we need to reset out physics frame counter.
secondCheck += dt;
if(secondCheck > 1.0f)
{
physicsFrames[threadNum] = 1;
secondCheck -= 1.0f;
}
else
{
physicsFrames[threadNum]++;
}
updateTimes[threadNum] = time;
// If we're running the physics function as a thread function then we
// need to keep it running, so this is the end of the while(programRunning) loop.
#if _USE_MT
}
#endif
}
// 3D simplex noise
float simplexnoise( float xin, float yin, float zin )
{
float n0, n1, n2, n3; // Noise contributions from the four corners
// Skew the input space to determine which simplex cell we're in
const float F3 = 1.0 / 3.0;
float s = ( xin + yin + zin ) * F3; // Very nice and simple skew factor for 3D
int i = floor( xin + s );
int j = floor( yin + s );
int k = floor( zin + s );
const float G3 = 1.0 / 6.0; // Very nice and simple unskew factor, too
float t = ( i + j + k ) * G3;
float X0 = i - t; // Unskew the cell origin back to (x,y,z) space
float Y0 = j - t;
float Z0 = k - t;
float x0 = xin - X0; // The x,y,z distances from the cell origin
float y0 = yin - Y0;
float z0 = zin - Z0;
// For the 3D case, the simplex shape is a slightly irregular tetrahedron.
// Determine which simplex we are in.
int i1, j1, k1; // Offsets for second corner of simplex in (i,j,k) coords
int i2, j2, k2; // Offsets for third corner of simplex in (i,j,k) coords
if( x0 >= y0 )
{
if( y0 >= z0 )
{
i1 = 1; j1 = 0; k1 = 0;
i2 = 1; j2 = 1; k2 = 0;
} // X Y Z order
else if( x0 >= z0 )
{
i1 = 1; j1 = 0; k1 = 0;
i2 = 1; j2 = 0; k2 = 1;
} // X Z Y order
else
{
i1 = 0; j1 = 0; k1 = 1;
i2 = 1; j2 = 0; k2 = 1;
} // Z X Y order
}
else
{ // x0<y0
if( y0 < z0 )
{
i1 = 0; j1 = 0; k1 = 1;
i2 = 0; j2 = 1; k2 = 1;
} // Z Y X order
else if( x0 < z0 )
{
i1 = 0; j1 = 1; k1 = 0;
i2 = 0; j2 = 1; k2 = 1;
} // Y Z X order
else
{
i1 = 0; j1 = 1; k1 = 0;
i2 = 1; j2 = 1; k2 = 0;
} // Y X Z order
}
// A step of (1,0,0) in (i,j,k) means a step of (1-c,-c,-c) in (x,y,z),
// a step of (0,1,0) in (i,j,k) means a step of (-c,1-c,-c) in (x,y,z), and
// a step of (0,0,1) in (i,j,k) means a step of (-c,-c,1-c) in (x,y,z), where
// c = 1/6.
float x1 = x0 - i1 + G3; // Offsets for second corner in (x,y,z) coords
float y1 = y0 - j1 + G3;
float z1 = z0 - k1 + G3;
float x2 = x0 - i2 + 2.0 * G3; // Offsets for third corner in (x,y,z) coords
float y2 = y0 - j2 + 2.0 * G3;
float z2 = z0 - k2 + 2.0 * G3;
float x3 = x0 - 1.0 + 3.0 * G3; // Offsets for last corner in (x,y,z) coords
float y3 = y0 - 1.0 + 3.0 * G3;
float z3 = z0 - 1.0 + 3.0 * G3;
// Work out the hashed gradient indices of the four simplex corners
int ii = i & 255;
int jj = j & 255;
int kk = k & 255;
int gi0 = perm[ii + perm[jj + perm[kk]]] % 12;
int gi1 = perm[ii + i1 + perm[jj + j1 + perm[kk + k1]]] % 12;
int gi2 = perm[ii + i2 + perm[jj + j2 + perm[kk + k2]]] % 12;
int gi3 = perm[ii + 1 + perm[jj + 1 + perm[kk + 1]]] % 12;
// Calculate the contribution from the four corners
float t0 = 0.6 - x0 * x0 - y0 * y0 - z0 * z0;
if( t0 < 0 )
n0 = 0.0;
else
{
t0 *= t0;
n0 = t0 * t0 * dot3( grad3[gi0], x0, y0, z0 );
}
float t1 = 0.6 - x1 * x1 - y1 * y1 - z1 * z1;
if( t1 < 0 )
n1 = 0.0;
else
{
t1 *= t1;
n1 = t1 * t1 * dot3( grad3[gi1], x1, y1, z1 );
}
float t2 = 0.6 - x2 * x2 - y2 * y2 - z2 * z2;
if( t2 < 0 )
n2 = 0.0;
else
{
t2 *= t2;
n2 = t2 * t2 * dot3( grad3[gi2], x2, y2, z2 );
}
float t3 = 0.6 - x3 * x3 - y3 * y3 - z3 * z3;
if( t3 < 0 )
n3 = 0.0;
else
{
t3 *= t3;
n3 = t3 * t3 * dot3( grad3[gi3], x3, y3, z3 );
}
// Add contributions from each corner to get the final noise value.
// The result is scaled to stay just inside [-1,1]
return 32.0 * ( n0 + n1 + n2 + n3 );
}
float dot(const float3 &a, const float3 &b)
{
return dot3(a, b);
}
float
noise3(float x, float y, float z)
{
int c, o1[3], o2[3], g[4], I, J, K;
float f[4], noise[4] = {0.0f, 0.0f, 0.0f, 0.0f};
float s = (x + y + z) * F3;
float i = floorf(x + s);
float j = floorf(y + s);
float k = floorf(z + s);
float t = (i + j + k) * G3;
float pos[4][3];
pos[0][0] = x - (i - t);
pos[0][1] = y - (j - t);
pos[0][2] = z - (k - t);
if (pos[0][0] >= pos[0][1]) {
if (pos[0][1] >= pos[0][2]) {
ASSIGN(o1, 1, 0, 0);
ASSIGN(o2, 1, 1, 0);
} else if (pos[0][0] >= pos[0][2]) {
ASSIGN(o1, 1, 0, 0);
ASSIGN(o2, 1, 0, 1);
} else {
ASSIGN(o1, 0, 0, 1);
ASSIGN(o2, 1, 0, 1);
}
} else {
if (pos[0][1] < pos[0][2]) {
ASSIGN(o1, 0, 0, 1);
ASSIGN(o2, 0, 1, 1);
} else if (pos[0][0] < pos[0][2]) {
ASSIGN(o1, 0, 1, 0);
ASSIGN(o2, 0, 1, 1);
} else {
ASSIGN(o1, 0, 1, 0);
ASSIGN(o2, 1, 1, 0);
}
}
for (c = 0; c <= 2; c++) {
pos[3][c] = pos[0][c] - 1.0f + 3.0f * G3;
pos[2][c] = pos[0][c] - o2[c] + 2.0f * G3;
pos[1][c] = pos[0][c] - o1[c] + G3;
}
I = (int) i & 255;
J = (int) j & 255;
K = (int) k & 255;
g[0] = PERM[I + PERM[J + PERM[K]]] % 12;
g[1] = PERM[I + o1[0] + PERM[J + o1[1] + PERM[o1[2] + K]]] % 12;
g[2] = PERM[I + o2[0] + PERM[J + o2[1] + PERM[o2[2] + K]]] % 12;
g[3] = PERM[I + 1 + PERM[J + 1 + PERM[K + 1]]] % 12;
for (c = 0; c <= 3; c++) {
f[c] = 0.6f - pos[c][0]*pos[c][0] - pos[c][1]*pos[c][1] - pos[c][2]*pos[c][2];
}
for (c = 0; c <= 3; c++) {
if (f[c] > 0) {
noise[c] = f[c]*f[c]*f[c]*f[c] * dot3(pos[c], GRAD3[g[c]]);
}
}
return (noise[0] + noise[1] + noise[2] + noise[3]) * 32.0f;
}
float trace(const ray *ray, const graphics_state *state, f3 *light, int max_depth){
*light = (f3){0,0,0};
f3 intersection, normal;
float t = intersect(ray, state, &intersection, &normal);
if(t < 0)
return -1;
/* shade; no shadows for now */
f3 specular = (f3){0.4f, 0.4f, 0.4f};
f3 diffuse = (f3){1,1,0.2f};
int j;
for(j = 0; j < state->light_count; j++){
f4 light_location = state->lights[j].location;
f3 light_direction = (f3){
light_location.x - intersection.x*light_location.w,
light_location.y - intersection.y*light_location.w,
light_location.z - intersection.z*light_location.w};
/* shadows -- these are slow.
struct ray light_ray;
light_ray.start = intersection;
light_ray.direction = light_direction;
f3 tmp_intersect, tmp_normal;
if(intersect(&light_ray, state, &tmp_intersect, &tmp_normal) > 0)
continue;*/
float cos_incident = dot3(&normal, &light_direction) / norm3(&light_direction);
if(cos_incident < 0)
cos_incident = 0;
cos_incident += 0.04f;
light->x += diffuse.x*cos_incident*state->lights[j].color.x;
light->y += diffuse.y*cos_incident*state->lights[j].color.y;
light->z += diffuse.z*cos_incident*state->lights[j].color.z;
}
/* recurse to get specular reflection */
float bounce = dot3(&ray->direction, &normal);
struct ray newRay;
newRay.direction = (f3){
ray->direction.x - 2*bounce*normal.x,
ray->direction.y - 2*bounce*normal.y,
ray->direction.z - 2*bounce*normal.z};
f3 newLight = (f3){0,0,0};
if(max_depth > 0){
newRay.start = intersection;
trace(&newRay, state, &newLight, max_depth-1);
}
/* special case: specular highlights */
for(j = 0; j < state->light_count; j++){
f4 light_location = state->lights[j].location;
f3 light_direction = (f3){
light_location.x - intersection.x*light_location.w,
light_location.y - intersection.y*light_location.w,
light_location.z - intersection.z*light_location.w};
float light_cos = dot3(&light_direction, &newRay.direction);
float thresh = 0.85;
if(light_cos > thresh){
float highlight = (light_cos - thresh)/(1-thresh);
highlight *= highlight * highlight * 3;
newLight.x += highlight*state->lights[j].color.x;
newLight.y += highlight*state->lights[j].color.y;
newLight.z += highlight*state->lights[j].color.z;
}
}
light->x += specular.x*newLight.x;
light->y += specular.y*newLight.y;
light->z += specular.z*newLight.z;
return t;
}
float vec3Len(const vec4& v) {
return sqrt(dot3(v ,v));
}
// Used as the F(t + dt, p(t + dt), v(t + dt)) equation.
FORCEINLINE void calcForce(Vector3 &forces, const float &springConst, const float &dampFactor, const float &overlap, const Vector3 &tangent, const Vector3 &newVelocity)
{
Vector3 vs = tangent * dot3(tangent, newVelocity);
forces -= (springConst * overlap * tangent) + (dampFactor * vs);
}
float vec3FastLen(const vec3& v) {
return dot3(v,v);
}