void computeBV<AABB>(const Convex& s, AABB& bv)
{
Vec3f R[3];
matMulMat(s.getRotation(), s.getLocalRotation(), R);
Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
AABB bv_;
for(int i = 0; i < s.num_points; ++i)
{
Vec3f new_p = matMulVec(R, s.points[i]) + T;
bv_ += new_p;
}
bv = bv_;
}
void computeBV<AABB>(const Sphere& s, AABB& bv)
{
Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
bv.max_ = T + Vec3f(s.radius, s.radius, s.radius);
bv.min_ = T + Vec3f(-s.radius, -s.radius, -s.radius);
}
void computeBV<AABB>(const Plane& s, AABB& bv)
{
Vec3f R[3];
matMulMat(s.getRotation(), s.getLocalRotation(), R);
Vec3f n = matMulVec(R, n);
AABB bv_;
if(n[1] == (BVH_REAL)0.0 && n[2] == (BVH_REAL)0.0)
{
// normal aligned with x axis
if(n[0] < 0) bv_.min_[0] = -s.d;
else if(n[0] > 0) bv_.max_[0] = s.d;
}
else if(n[0] == (BVH_REAL)0.0 && n[2] == (BVH_REAL)0.0)
{
// normal aligned with y axis
if(n[1] < 0) bv_.min_[1] = -s.d;
else if(n[1] > 0) bv_.max_[1] = s.d;
}
else if(n[0] == (BVH_REAL)0.0 && n[1] == (BVH_REAL)0.0)
{
// normal aligned with z axis
if(n[2] < 0) bv_.min_[2] = -s.d;
else if(n[2] > 0) bv_.max_[2] = s.d;
}
bv = bv_;
}
void computeBV<OBB>(const Sphere& s, OBB& bv)
{
Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
bv.To = T;
bv.axis[0] = Vec3f(1, 0, 0);
bv.axis[1] = Vec3f(0, 1, 0);
bv.axis[2] = Vec3f(0, 0, 1);
bv.extent = Vec3f(s.radius, s.radius, s.radius);
}
void computeBV<OBB>(const Convex& s, OBB& bv)
{
Vec3f R[3];
matMulMat(s.getRotation(), s.getLocalRotation(), R);
Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
fit(s.points, s.num_points, bv);
Vec3f axis[3];
axis[0] = matMulVec(R, bv.axis[0]);
axis[1] = matMulVec(R, bv.axis[1]);
axis[2] = matMulVec(R, bv.axis[2]);
bv.axis[0] = axis[0];
bv.axis[1] = axis[1];
bv.axis[2] = axis[2];
bv.To = matMulVec(R, bv.To) + T;
}
void computeBV<OBB>(const Cylinder& s, OBB& bv)
{
Vec3f R[3];
matMulMat(s.getRotation(), s.getLocalRotation(), R);
Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
bv.To = T;
bv.axis[0] = Vec3f(R[0][0], R[1][0], R[2][0]);
bv.axis[1] = Vec3f(R[0][1], R[1][1], R[2][1]);
bv.axis[2] = Vec3f(R[0][2], R[1][2], R[2][2]);
bv.extent = Vec3f(s.radius, s.radius, s.lz / 2);
}
void computeBV<OBB>(const Box& s, OBB& bv)
{
Vec3f R[3];
matMulMat(s.getRotation(), s.getLocalRotation(), R);
Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
bv.To = T;
bv.axis[0] = Vec3f(R[0][0], R[1][0], R[2][0]);
bv.axis[1] = Vec3f(R[0][1], R[1][1], R[2][1]);
bv.axis[2] = Vec3f(R[0][2], R[1][2], R[2][2]);
bv.extent = s.side * (BVH_REAL)0.5;
}
void fpscont(Camera* cam) {
// Rip out position
//
float vec_pos[4];
float* mat_world = cam->mat_world;
memcpy(vec_pos, &mat_world[12], sizeof(vec_pos));
memset(&mat_world[12], 0, sizeof(float) * 3);
// Mouse-look
//
short center_x = display.getWidth()/2;
short center_y = display.getHeight()/2;
short delta_x = mouse.pos[0] - center_x;
short delta_y = mouse.pos[1] - center_y;
mouse.setPos(center_x, center_y);
float mat_rotx[16];
matRot(mat_rotx, delta_y * -0.001f, 0);
matMul(mat_world, mat_rotx, mat_world);
float mat_roty[16];
matRot(mat_roty, delta_x * -0.001f, 1);
matMul(mat_roty, mat_world, mat_world);
// Movement
//
char up_down = keyboard.keyDown('w') - keyboard.keyDown('s');
char right_left = keyboard.keyDown('d') - keyboard.keyDown('a');
float speed = 0.5;
float vec_vel[4];
vec_vel[0] = right_left * speed;
vec_vel[1] = 0;
vec_vel[2] = -up_down * speed;
vec_vel[3] = 1;
matMulVec(mat_world, vec_vel, vec_vel);
vecAdd(vec_pos, vec_vel, vec_pos);
vec_pos[3] = 1;
memcpy(cam->mat_view, cam->mat_world, sizeof(float) * 16);
memcpy(&cam->mat_world[12], vec_pos, sizeof(vec_pos));
cameraWorldToView(cam);
}
void computeBV<OBB>(const Plane& s, OBB& bv)
{
Vec3f R[3];
matMulMat(s.getRotation(), s.getLocalRotation(), R);
Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
// generate other two axes orthonormal to plane normal
const Vec3f& w = s.n;
Vec3f u, v;
float inv_length;
if(fabs(w[0]) >= fabs(w[1]))
{
inv_length = 1.0 / sqrt(w[0] * w[0] + w[2] * w[2]);
u[0] = -w[2] * inv_length;
u[1] = 0;
u[2] = w[0] * inv_length;
v[0] = w[1] * u[2];
v[1] = w[2] * u[0] - w[0] * u[2];
v[2] = -w[1] * u[0];
}
else
{
inv_length = 1.0 / sqrt(w[1] * w[1] + w[2] * w[2]);
u[0] = 0;
u[1] = w[2] * inv_length;
u[2] = -w[1] * inv_length;
v[0] = w[1] * u[2] - w[2] * u[1];
v[1] = -w[0] * u[2];
v[2] = w[0] * u[1];
}
bv.axis[0] = w;
bv.axis[1] = u;
bv.axis[2] = v;
bv.extent = Vec3f(0, std::numeric_limits<BVH_REAL>::max(), std::numeric_limits<BVH_REAL>::max());
Vec3f p = s.n * s.d;
bv.To = matMulVec(R, p) + T;
}
void computeBV<AABB>(const Cylinder& s, AABB& bv)
{
Vec3f R[3];
matMulMat(s.getRotation(), s.getLocalRotation(), R);
Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
BVH_REAL x_range = fabs(R[0][0] * s.radius) + fabs(R[0][1] * s.radius) + 0.5 * fabs(R[0][2] * s.lz);
BVH_REAL y_range = fabs(R[1][0] * s.radius) + fabs(R[1][1] * s.radius) + 0.5 * fabs(R[1][2] * s.lz);
BVH_REAL z_range = fabs(R[2][0] * s.radius) + fabs(R[2][1] * s.radius) + 0.5 * fabs(R[2][2] * s.lz);
bv.max_ = T + Vec3f(x_range, y_range, z_range);
bv.min_ = T + Vec3f(-x_range, -y_range, -z_range);
}
void computeBV<AABB>(const Box& s, AABB& bv)
{
Vec3f R[3];
matMulMat(s.getRotation(), s.getLocalRotation(), R);
Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
BVH_REAL x_range = 0.5 * (fabs(R[0][0] * s.side[0]) + fabs(R[0][1] * s.side[1]) + fabs(R[0][2] * s.side[2]));
BVH_REAL y_range = 0.5 * (fabs(R[1][0] * s.side[0]) + fabs(R[1][1] * s.side[1]) + fabs(R[1][2] * s.side[2]));
BVH_REAL z_range = 0.5 * (fabs(R[2][0] * s.side[0]) + fabs(R[2][1] * s.side[1]) + fabs(R[2][2] * s.side[2]));
bv.max_ = T + Vec3f(x_range, y_range, z_range);
bv.min_ = T + Vec3f(-x_range, -y_range, -z_range);
}
void triangleVertexShading(JmJob *data)
{
VertexJob v;
dmaBlockGet(&v, (uintptr_t)data->p1, sizeof(VertexJob), DmaTag);
Vec halfSizeAdd = vec(0.5f * sw, 0.5f * sh, 0, 1);
Vec halfSizeMul = vec(0.5f * sw, -0.5f * sh, 1, 0);
Vec zero = vec(0);
Mat transform;
matMul(&transform, v.projection, v.world);
while(true)
{
unsigned int k = interlockedExchangeAdd(v.counter, VerticesAtATime);
if(k >= v.end) break;
unsigned int end = std::min(k + VerticesAtATime, v.end);
unsigned int num = end - k;
unsigned int numtris = num / 3;
#ifndef PLATFORM_PS3_SPU
Triangle3D vertexShadingTris[TriangleAtATime];
#endif
dmaBlockGet(vertexShadingTris, (uintptr_t)&v.input[k], numtris * sizeof(Triangle3D), DmaTag);
for(unsigned int u = 0; u < numtris; u++)
{
const Triangle3D &tri = vertexShadingTris[u];
Triangle3D triOut[5];
Vec vtx[8];
int numTrisOut = 1;
if(g_EnableBackfaceCulling)
{
// We can't do the backface culling using a determinant of a 2x2 matrix
// in screen space because then we would have 'holes' in the output data
// therefor it happens here, in wordspace.
Vec e1 = vecSub(tri.p3, tri.p1);
Vec e2 = vecSub(tri.p2, tri.p1);
Vec n = vecCross(e1, e2);
Vec a = vecDot(v.camerapos, n);
if(vecGetElem(a, VecComponent::X) > 0) continue;
}
// perspective project
matMulVec(&triOut[0].p1, transform, tri.p1);
matMulVec(&triOut[0].p2, transform, tri.p2);
matMulVec(&triOut[0].p3, transform, tri.p3);
// cull against znear
Vec m1 = vecCmpLE(vecSplat<VecComponent::Z>(triOut[0].p1), zero);
Vec m2 = vecCmpLE(vecSplat<VecComponent::Z>(triOut[0].p2), zero);
Vec m3 = vecCmpLE(vecSplat<VecComponent::Z>(triOut[0].p3), zero);
Vec c2 = vecAnd(vecAnd(m1, m2), m3);
#ifdef PLATFORM_PS3
vec_uint4 ones = (vec_uint4){0xffffffff,0xffffffff,0xffffffff,0xffffffff};
if(vec_all_eq((vec_uint4)c2, ones)) continue;
#else
int result = vecMaskToInt(c2);
if(result == 15) continue; // discard, all behind nearz
#endif
#if 1
// clip triangles that intersect znear
static const int NumVerticesInATriangle = 3;
int numVertsOut = triangleClipToPlane(
(Vec*)&triOut[0],
vtx,
NumVerticesInATriangle,
vec(0, 0, 1, 0)
);
// Very simple triangulation routine
numTrisOut = 0;
for(int i = 2; i < numVertsOut; i++)
{
triOut[numTrisOut].p1 = vtx[0];
triOut[numTrisOut].p2 = vtx[i];
triOut[numTrisOut].p3 = vtx[i - 1];
numTrisOut++;
}
#endif
for(int i = 0; i < numTrisOut; i++)
{
// perspective divide
triOut[i].p1 = vecMul(triOut[i].p1, vecRcp(vecSplat<VecComponent::W>(triOut[i].p1)));
triOut[i].p2 = vecMul(triOut[i].p2, vecRcp(vecSplat<VecComponent::W>(triOut[i].p2)));
triOut[i].p3 = vecMul(triOut[i].p3, vecRcp(vecSplat<VecComponent::W>(triOut[i].p3)));
// transform to screen space
Vec r1 = vecMadd(triOut[i].p1, halfSizeMul, halfSizeAdd);
Vec r2 = vecMadd(triOut[i].p2, halfSizeMul, halfSizeAdd);
Vec r3 = vecMadd(triOut[i].p3, halfSizeMul, halfSizeAdd);
#ifdef PLATFORM_PS3_SPU
Triangle3DSetup &r = setup[s][sidx];
#else
Triangle3DSetup r;
#endif
memcpy(&r.x1, &r1, sizeof(float) * 3);
memcpy(&r.x2, &r2, sizeof(float) * 3);
memcpy(&r.x3, &r3, sizeof(float) * 3);
// deltas
r.dx1 = r.x1 - r.x2;
r.dx2 = r.x2 - r.x3;
r.dx3 = r.x3 - r.x1;
r.dy1 = r.y1 - r.y2;
r.dy2 = r.y2 - r.y3;
r.dy3 = r.y3 - r.y1;
#ifdef PLATFORM_PS3_SPU
sidx++;
if(sidx >= MaxSetupBuffered)
{
dmaWaitAll(1 << DmaListTag);
unsigned int l = interlockedExchangeAdd(v.outputCnt, sidx);
for(unsigned int u = 0; u < sidx; u++)
{
setuplist[u].notify = 0;
setuplist[u].reserved = 0;
setuplist[u].size = sizeof(Triangle3DSetup);
setuplist[u].eal = (uintptr_t)&v.output[u + l];
}
cellDmaListPut(setup[s], 0, setuplist, sizeof(setuplist), DmaListTag, 0, 0);
sidx = 0;
s ^= 1;
}
#else
unsigned int l = interlockedExchangeAdd(v.outputCnt, 1);
if(l >= MaxTrianglesDrawn) { interlockedExchangeSub(v.outputCnt, 1); break; }
else dmaBlockPut(&r, (uintptr_t)&v.output[l], sizeof(Triangle3DSetup), DmaTag);
#endif
}
}
}
#ifdef PLATFORM_PS3_SPU
if(sidx > 0)
{
dmaWaitAll(1 << DmaListTag);
unsigned int l = interlockedExchangeAdd(v.outputCnt, sidx);
for(unsigned int u = 0; u < sidx; u++)
{
setuplist[u].notify = 0;
setuplist[u].reserved = 0;
setuplist[u].size = sizeof(Triangle3DSetup);
setuplist[u].eal = (uintptr_t)&v.output[u + l];
}
cellDmaListPut(setup[s], 0, setuplist, sidx * sizeof(CellDmaListElement), DmaListTag + 1, 0, 0);
dmaWaitAll(1 << (DmaListTag + 1));
}
#endif
}
bool PhysicsWorld::Intersects(const StaticPhysicsEntity &s, const DynamicPhysicsEntity &d, Vec &outNormal, Vec &outDir)
{
float dmin = 0, radiusSquared;
Vec min, max, center, scale;
min = vec(-1, -1, -1, 0);
max = vec(1, 1, 1, 0);
scale = vec(1, 1, 1, 0);
Mat m, sc;
center = entGetTransform(d.ent)->GetPosition();
entGetTransform(s.ent)->GetMatrix(&m);
entGetTransform(d.ent)->GetMatrix(&sc);
// scale the unit vector and take the largest component
// as the radius for the sphere; as long as we don't do
// silly rotations we should be ok
matMulVec(&scale, sc, scale);
radiusSquared = vecMax(scale);
radiusSquared *= radiusSquared;
// we don't have an inv matrix of anything so instead
// we transform everything to worldspace and do the
// calculations there
// rot + scale
matMulVec(&min, m, min);
matMulVec(&max, m, max);
// pos
Vec p = entGetTransform(s.ent)->GetPosition();
min = vecAdd(min, p);
max = vecAdd(max, p);
// the actual intersection test (don't test w)
for(int i = 0; i < 3; i++)
{
float C = vecGetElem(center, i);
float Bmin = vecGetElem(min, i);
float Bmax = vecGetElem(max, i);
if(C < Bmin) dmin += square(C - Bmin);
else if(C > Bmax) dmin += square(C - Bmax);
}
if(dmin <= radiusSquared)
{
Ray r;
r.o = center;
r.d = vecSub(p, center);
r.d = vecNormalize(r.d);
float dist;
bool hit = RayBoxIntersection(r, min, max, dist, outNormal);
outDir = vecMul(r.d, vec(dist));
return hit;
}
return false;
}
