Point3 knot_fn(float s, float t)
{
const float a = 0.5f;
const float b = 0.3f;
const float c = 0.5f;
const float d = 0.1f;
const float u = (1 - s) * 2 * PARG_TWOPI;
const float v = t * PARG_TWOPI;
const float r = a + b * cos(1.5f * u);
const float x = r * cos(u);
const float y = r * sin(u);
const float z = c * sin(1.5f * u);
Vector3 dv;
dv.x =
-1.5f * b * sin(1.5f * u) * cos(u) - (a + b * cos(1.5f * u)) * sin(u);
dv.y =
-1.5f * b * sin(1.5f * u) * sin(u) + (a + b * cos(1.5f * u)) * cos(u);
dv.z = 1.5f * c * cos(1.5f * u);
Vector3 q = V3Normalize(dv);
Vector3 qvn = V3Normalize((Vector3){q.y, -q.x, 0});
Vector3 ww = V3Cross(q, qvn);
Point3 range;
range.x = x + d * (qvn.x * cos(v) + ww.x * sin(v));
range.y = y + d * (qvn.y * cos(v) + ww.y * sin(v));
range.z = z + d * ww.z * sin(v);
return range;
}
parg_mesh* parg_mesh_knot(int slices, int stacks, float major, float minor)
{
parg_mesh* surf = malloc(sizeof(struct parg_mesh_s));
float ds = 1.0f / slices;
float dt = 1.0f / stacks;
int vertexCount = slices * stacks * 3;
int vertexStride = sizeof(float) * 3;
surf->coords =
parg_buffer_alloc(vertexCount * vertexStride, PARG_GPU_ARRAY);
surf->normals =
parg_buffer_alloc(vertexCount * vertexStride, PARG_GPU_ARRAY);
surf->uvs = 0;
Point3* position = (Point3*) parg_buffer_lock(surf->coords, PARG_WRITE);
Vector3* normal = (Vector3*) parg_buffer_lock(surf->normals, PARG_WRITE);
for (float s = 0; s < 1 - ds / 2; s += ds) {
for (float t = 0; t < 1 - dt / 2; t += dt) {
const float E = 0.01f;
Point3 p = knot_fn(s, t);
Vector3 u = P3Sub(knot_fn(s + E, t), p);
Vector3 v = P3Sub(knot_fn(s, t + E), p);
Vector3 n = V3Normalize(V3Cross(u, v));
*position++ = p;
*normal++ = n;
}
}
parg_buffer_unlock(surf->coords);
parg_buffer_unlock(surf->normals);
surf->ntriangles = slices * stacks * 2;
int indexCount = surf->ntriangles * 3;
surf->indices = parg_buffer_alloc(indexCount * 2, PARG_GPU_ELEMENTS);
uint16_t* index = (uint16_t*) parg_buffer_lock(surf->indices, PARG_WRITE);
int v = 0;
for (int i = 0; i < slices - 1; i++) {
for (int j = 0; j < stacks; j++) {
int next = (j + 1) % stacks;
*index++ = v + next + stacks;
*index++ = v + next;
*index++ = v + j;
*index++ = v + j;
*index++ = v + j + stacks;
*index++ = v + next + stacks;
}
v += stacks;
}
for (int j = 0; j < stacks; j++) {
int next = (j + 1) % stacks;
*index++ = next;
*index++ = v + next;
*index++ = v + j;
*index++ = v + j;
*index++ = j;
*index++ = next;
}
parg_buffer_unlock(surf->indices);
return surf;
}
Point3 *NormalToFace(Face *f, Point3 *n)
{
Point3 t1,t2;
V3Sub(f->v[1],f->v[0],&t1);
V3Sub(f->v[2],f->v[0],&t2);
V3Cross(&t1,&t2,n);
return n;
}
Point3 *NormalToFace(Point3 *v, Face *f, Point3 *n)
{
Point3 t1,t2;
V3Sub(&(v[f->v[1]]),&(v[f->v[0]]),&t1);
V3Sub(&(v[f->v[2]]),&(v[f->v[0]]),&t2);
V3Cross(&t1,&t2,n);
return n;
}
void PxcLtbComputeJv(Vec3V* jv, const PxcFsData& m, const PxcSIMDSpatial* velocity)
{
typedef PxcArticulationFnsSimd<PxcArticulationFnsSimdBase> Fns;
const PxcLtbRow* rows = getLtbRows(m);
const PxcFsRow* fsRows = getFsRows(m);
const PxcFsJointVectors* jointVectors = getJointVectors(m);
PX_UNUSED(rows);
PX_UNUSED(fsRows);
for(PxU32 i=1;i<m.linkCount;i++)
{
PxcSIMDSpatial pv = velocity[m.parent[i]], v = velocity[i];
Vec3V parentOffset = V3Add(jointVectors[i].jointOffset, jointVectors[i].parentOffset);
Vec3V k0v = V3Add(pv.linear, V3Cross(pv.angular, parentOffset)),
k1v = V3Add(v.linear, V3Cross(v.angular,jointVectors[i].jointOffset));
jv[i] = V3Sub(k0v, k1v);
}
}
PX_FORCE_INLINE PxcSIMDSpatial propagateDrivenImpulse(const PxcFsRow& row,
const PxcFsJointVectors& jv,
Vec3V& SZMinusQ,
const PxcSIMDSpatial& Z,
const Vec3V& Q)
{
typedef PxcArticulationFnsSimd<PxcArticulationFnsSimdBase> Fns;
SZMinusQ = V3Sub(V3Add(Z.angular, V3Cross(Z.linear,jv.jointOffset)), Q);
PxcSIMDSpatial result = Fns::translateForce(jv.parentOffset, Z - Fns::axisMultiply(row.DSI, SZMinusQ));
return result;
}
Plane *CalcPlane(Vector3 *p0, Vector3 *p1, Vector3 *p2, Plane *p )
{
Vector3 v1,v2;
Vector3 *N;
N=&(p->N);
V3Sub(p1,p0,&v1);
V3Sub(p2,p0,&v2);
V3Cross(&v1,&v2,N);
if(N->x==0.0 && N->y==0.0 && N->z==0.0) return NULL;
V3Normalize(N);
p->off=V3Dot(N,p0);
return p;
}
void
Example1()
{
Vector4 *a, *b, *c, r;
Vector3 *a1, *b1, r1;
a = V4New(1.0, 0.0, 0.0, 0.0);
b = V4New(0.0, 1.0, 0.0, 0.0);
c = V4New(0.0, 0.0, 1.0, 0.0);
a1 = V3New(1.0, 0.0, 0.0);
b1 = V3New(0.0, 1.0, 0.0);
V3Cross(a1, b1, &r1);
V4Cross(a, b, c, &r);
printf("(%lf, %lf, %lf)\n", r1.x, r1.y, r1.z);
printf("(%lf, %lf, %lf, %lf)\n", r.x, r.y, r.z, r.w);
}
/*!
\brief Define plane
\param p1,p2,p3 three point on plane
\param[out] plane plane defintion
\return 1
*/
int P3toPlane(Point3 p1, Point3 p2, Point3 p3, float *plane)
{
Point3 v1, v2, norm;
v1[X] = p1[X] - p3[X];
v1[Y] = p1[Y] - p3[Y];
v1[Z] = p1[Z] - p3[Z];
v2[X] = p2[X] - p3[X];
v2[Y] = p2[Y] - p3[Y];
v2[Z] = p2[Z] - p3[Z];
V3Cross(v1, v2, norm);
plane[X] = norm[X];
plane[Y] = norm[Y];
plane[Z] = norm[Z];
plane[W] = -p3[X] * norm[X] - p3[Y] * norm[Y] - p3[Z] * norm[Z];
return (1);
}
void SetupPolygon(PolygonData *ply)
{
Vec3 V1, V2, N;
float *pts;
assert(ply != NULL);
assert(ply->npts >= 3);
pts = ply->pts;
assert(pts != NULL);
/* Compute plane normal. (based on first three vertices) */
V1.x = pts[3] - pts[0];
V2.x = pts[6] - pts[0];
V1.y = pts[4] - pts[1];
V2.y = pts[7] - pts[1];
V1.z = pts[5] - pts[2];
V2.z = pts[8] - pts[2];
V3Cross(&N, &V1, &V2);
V3Normalize(&N);
ply->nx = (float)N.x;
ply->ny = (float)N.y;
ply->nz = (float)N.z;
/* Determine axis of greatest projection. */
if(fabs(N.x) > fabs(N.z))
{
if(fabs(N.x) > fabs(N.y))
ply->axis = X_AXIS;
else
ply->axis = Y_AXIS;
}
else if(fabs(N.y) > fabs(N.z))
ply->axis = Y_AXIS;
else
ply->axis = Z_AXIS;
}
parg_mesh* parg_mesh_torus(int slices, int stacks, float major, float minor)
{
parg_mesh* surf = malloc(sizeof(struct parg_mesh_s));
float dphi = PARG_TWOPI / stacks;
float dtheta = PARG_TWOPI / slices;
int vertexCount = slices * stacks * 3;
int vertexStride = sizeof(float) * 3;
surf->coords =
parg_buffer_alloc(vertexCount * vertexStride, PARG_GPU_ARRAY);
surf->uvs = 0;
Point3* position = (Point3*) parg_buffer_lock(surf->coords, PARG_WRITE);
for (int slice = 0; slice < slices; slice++) {
float theta = slice * dtheta;
for (int stack = 0; stack < stacks; stack++) {
float phi = stack * dphi;
*position++ = torus_fn(major, minor, phi, theta);
}
}
parg_buffer_unlock(surf->coords);
surf->normals =
parg_buffer_alloc(vertexCount * vertexStride, PARG_GPU_ARRAY);
Vector3* normal = (Vector3*) parg_buffer_lock(surf->normals, PARG_WRITE);
for (int slice = 0; slice < slices; slice++) {
float theta = slice * dtheta;
for (int stack = 0; stack < stacks; stack++) {
float phi = stack * dphi;
Point3 p = torus_fn(major, minor, phi, theta);
Point3 p1 = torus_fn(major, minor, phi, theta + 0.01);
Point3 p2 = torus_fn(major, minor, phi + 0.01, theta);
Vector3 du = P3Sub(p2, p);
Vector3 dv = P3Sub(p1, p);
*normal = V3Normalize(V3Cross(du, dv));
++normal;
}
}
parg_buffer_unlock(surf->normals);
surf->ntriangles = slices * stacks * 2;
int indexCount = surf->ntriangles * 3;
surf->indices = parg_buffer_alloc(indexCount * 2, PARG_GPU_ELEMENTS);
uint16_t* index = (uint16_t*) parg_buffer_lock(surf->indices, PARG_WRITE);
int v = 0;
for (int i = 0; i < slices - 1; i++) {
for (int j = 0; j < stacks; j++) {
int next = (j + 1) % stacks;
*index++ = v + next + stacks;
*index++ = v + next;
*index++ = v + j;
*index++ = v + j;
*index++ = v + j + stacks;
*index++ = v + next + stacks;
}
v += stacks;
}
for (int j = 0; j < stacks; j++) {
int next = (j + 1) % stacks;
*index++ = next;
*index++ = v + next;
*index++ = v + j;
*index++ = v + j;
*index++ = j;
*index++ = next;
}
parg_buffer_unlock(surf->indices);
return surf;
}
void PxcArticulationHelper::getImpulseSelfResponse(const PxcFsData& matrix,
PxU32 linkID0,
const PxcSIMDSpatial& impulse0,
PxcSIMDSpatial& deltaV0,
PxU32 linkID1,
const PxcSIMDSpatial& impulse1,
PxcSIMDSpatial& deltaV1)
{
PX_ASSERT(linkID0 != linkID1);
const PxcFsRow* rows = getFsRows(matrix);
const PxcFsRowAux* aux = getAux(matrix);
const PxcFsJointVectors* jointVectors = getJointVectors(matrix);
PX_UNUSED(aux);
PxcSIMDSpatial& dV0 = deltaV0,
& dV1 = deltaV1;
// standard case: parent-child limit
if(matrix.parent[linkID1] == linkID0)
{
const PxcFsRow& r = rows[linkID1];
const PxcFsJointVectors& j = jointVectors[linkID1];
Vec3V lZ = V3Neg(impulse1.linear),
aZ = V3Neg(impulse1.angular);
Vec3V sz = V3Add(aZ, V3Cross(lZ, j.jointOffset));
lZ = V3Sub(lZ, V3ScaleAdd(r.DSI[0].linear, V3GetX(sz), V3ScaleAdd(r.DSI[1].linear, V3GetY(sz), V3Scale(r.DSI[2].linear, V3GetZ(sz)))));
aZ = V3Sub(aZ, V3ScaleAdd(r.DSI[0].angular, V3GetX(sz), V3ScaleAdd(r.DSI[1].angular, V3GetY(sz), V3Scale(r.DSI[2].angular, V3GetZ(sz)))));
aZ = V3Add(aZ, V3Cross(j.parentOffset, lZ));
lZ = V3Sub(impulse0.linear, lZ);
aZ = V3Sub(impulse0.angular, aZ);
dV0 = getImpulseResponseSimd(matrix, linkID0, lZ, aZ);
Vec3V aV = dV0.angular;
Vec3V lV = V3Sub(dV0.linear, V3Cross(j.parentOffset, aV));
Vec3V n = V3Add(V3Merge(V3Dot(r.DSI[0].linear, lV), V3Dot(r.DSI[1].linear, lV), V3Dot(r.DSI[2].linear, lV)),
V3Merge(V3Dot(r.DSI[0].angular, aV), V3Dot(r.DSI[1].angular, aV), V3Dot(r.DSI[2].angular, aV)));
n = V3Add(n, M33MulV3(r.D, sz));
lV = V3Sub(lV, V3Cross(j.jointOffset, n));
aV = V3Sub(aV, n);
dV1 = PxcSIMDSpatial(lV, aV);
}
else
getImpulseResponseSlow(matrix, linkID0, impulse0, deltaV0, linkID1, impulse1, deltaV1);
#if PXC_ARTICULATION_DEBUG_VERIFY
PxcSIMDSpatial dV0_, dV1_;
PxcFsGetImpulseSelfResponse(matrix, linkID0, impulse0, dV0_, linkID1, impulse1, dV1_);
PX_ASSERT(almostEqual(dV0_, dV0, 1e-3f));
PX_ASSERT(almostEqual(dV1_, dV1, 1e-3f));
#endif
}
static MeshPod CreateTrefoil()
{
const int Slices = 256;
const int Stacks = 32;
const int VertexCount = Slices * Stacks;
const int IndexCount = VertexCount * 6;
MeshPod mesh;
glGenVertexArrays(1, &mesh.Vao);
glBindVertexArray(mesh.Vao);
// Create a buffer with interleaved positions and normals
if (true) {
Vertex verts[VertexCount];
Vertex* pVert = &verts[0];
float ds = 1.0f / Slices;
float dt = 1.0f / Stacks;
// The upper bounds in these loops are tweaked to reduce the
// chance of precision error causing an incorrect # of iterations.
for (float s = 0; s < 1 - ds / 2; s += ds) {
for (float t = 0; t < 1 - dt / 2; t += dt) {
const float E = 0.01f;
Vector3 p = EvaluateTrefoil(s, t);
Vector3 u = V3Sub(EvaluateTrefoil(s + E, t), p);
Vector3 v = V3Sub(EvaluateTrefoil(s, t + E), p);
Vector3 n = V3Normalize(V3Cross(u, v));
pVert->Position = p;
pVert->Normal = n;
++pVert;
}
}
pezCheck(pVert - &verts[0] == VertexCount, "Tessellation error.");
GLuint vbo;
GLsizeiptr size = sizeof(verts);
const GLvoid* data = &verts[0].Position.x;
GLenum usage = GL_STATIC_DRAW;
glGenBuffers(1, &vbo);
glBindBuffer(GL_ARRAY_BUFFER, vbo);
glBufferData(GL_ARRAY_BUFFER, size, data, usage);
}
// Create a buffer of 16-bit indices
if (true) {
GLushort inds[IndexCount];
GLushort* pIndex = &inds[0];
GLushort n = 0;
for (GLushort i = 0; i < Slices; i++) {
for (GLushort j = 0; j < Stacks; j++) {
*pIndex++ = (n + j + Stacks) % VertexCount;
*pIndex++ = n + (j + 1) % Stacks;
*pIndex++ = n + j;
*pIndex++ = (n + (j + 1) % Stacks + Stacks) % VertexCount;
*pIndex++ = (n + (j + 1) % Stacks) % VertexCount;
*pIndex++ = (n + j + Stacks) % VertexCount;
}
n += Stacks;
}
pezCheck(n == VertexCount, "Tessellation error.");
pezCheck(pIndex - &inds[0] == IndexCount, "Tessellation error.");
GLuint handle;
GLsizeiptr size = sizeof(inds);
const GLvoid* data = &inds[0];
GLenum usage = GL_STATIC_DRAW;
glGenBuffers(1, &handle);
glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, handle);
glBufferData(GL_ELEMENT_ARRAY_BUFFER, size, data, usage);
}
mesh.VertexCount = VertexCount;
mesh.IndexCount = IndexCount;
glVertexAttribPointer(a("Position"), 3, GL_FLOAT, GL_FALSE, 24, 0);
glVertexAttribPointer(a("Normal"), 3, GL_FLOAT, GL_FALSE, 24, offset(12));
glEnableVertexAttribArray(a("Position"));
glEnableVertexAttribArray(a("Normal"));
pezCheck(OpenGLError);
return mesh;
}
void SetupColorTriangle(ColorTriangleData *tri)
{
Vec3 V1, V2, N;
double len;
int i;
/* Compute plane normal. */
V1.x = tri->pts[3] - tri->pts[0];
V2.x = tri->pts[6] - tri->pts[0];
V1.y = tri->pts[4] - tri->pts[1];
V2.y = tri->pts[7] - tri->pts[1];
V1.z = tri->pts[5] - tri->pts[2];
V2.z = tri->pts[8] - tri->pts[2];
V3Cross(&N, &V1, &V2);
V3Normalize(&N);
tri->pnorm[0] = (float)N.x;
tri->pnorm[1] = (float)N.y;
tri->pnorm[2] = (float)N.z;
/* Normalize the vertex normals. */
for(i = 0; i < 9; i += 3)
{
if((tri->norms[i] * N.x +
tri->norms[i+1] * N.y +
tri->norms[i+2] * N.z) < 0.0)
{
tri->norms[i] = - tri->norms[i];
tri->norms[i+1] = - tri->norms[i+1];
tri->norms[i+2] = - tri->norms[i+2];
}
len = sqrt(tri->norms[i] * tri->norms[i] +
tri->norms[i+1] * tri->norms[i+1] +
tri->norms[i+2] * tri->norms[i+2]);
if(len > EPSILON)
{
tri->norms[i] /= (float)len;
tri->norms[i+1] /= (float)len;
tri->norms[i+2] /= (float)len;
}
else /* Use the plane normal for bad vertex normals. */
{
tri->norms[i] = (float)N.x;
tri->norms[i+1] = (float)N.y;
tri->norms[i+2] = (float)N.z;
}
}
/* Determine axis of greatest projection. */
if(fabs(N.x) > fabs(N.z))
{
if(fabs(N.x) > fabs(N.y))
tri->axis = X_AXIS;
else if(fabs(N.y) > fabs(N.z))
tri->axis = Y_AXIS;
else
tri->axis = Z_AXIS;
}
else if(fabs(N.y) > fabs(N.z))
tri->axis = Y_AXIS;
else
tri->axis = Z_AXIS;
}
void eval_vcross(Expr *expr)
{
expr->l->fn(expr->l);
expr->r->fn(expr->r);
V3Cross(&expr->v, &expr->l->v, &expr->r->v);
}
/*
* LoadSlopeInfo: Load data for sloped surface descriptions and generate
* runtime representation of the data describing the texture coordinates.
* Return pointer to new SlopeData record on success, NULL on error
*/
SlopeData *LoadSlopeInfo(file_node *f) {
int size;
long txt_angle;
long index;
SlopeData *new_slope;
Vector3D texture_orientation;
Vector3D v1,v2;
char junk[6];
// create a new slope record
size = sizeof(SlopeData);
new_slope = (SlopeData *) SafeMalloc(size);
memset(new_slope, 0, size);
// load coefficients of plane equation
if (CliMappedFileRead(f, &new_slope->plane.a, 4) != 4) return (SlopeData *)NULL;
if (CliMappedFileRead(f, &new_slope->plane.b, 4) != 4) return (SlopeData *)NULL;
if (CliMappedFileRead(f, &new_slope->plane.c, 4) != 4) return (SlopeData *)NULL;
if (CliMappedFileRead(f, &new_slope->plane.d, 4) != 4) return (SlopeData *)NULL;
// dprintf("loaded equation a = %d, b = %d, c = %d, d = %d\n", new_slope->plane.a, new_slope->plane.b, new_slope->plane.c, new_slope->plane.d);
if (new_slope->plane.c == 0) {
debug(("Error: loaded plane equation equal to a vertical slope\n"));
// punt on error and stick in non crashing values (use assert instead?)
new_slope->plane.a = 0;
new_slope->plane.b = 0;
new_slope->plane.c = 1024;
new_slope->plane.d = 0;
}
// load x & y of texture origin
if (CliMappedFileRead(f, &new_slope->p0.x, 4) != 4) return (SlopeData *)NULL;
if (CliMappedFileRead(f, &new_slope->p0.y, 4) != 4) return (SlopeData *)NULL;
// calculate z of texture origin from x, y, and plane equation
new_slope->p0.z = (-new_slope->plane.a*new_slope->p0.x - new_slope->plane.b*new_slope->p0.y - new_slope->plane.d)/new_slope->plane.c;
// load in texture angle - this is planar angle between x axis of texture & x axis of world
if (CliMappedFileRead(f, &txt_angle, 4) != 4) return (SlopeData *)NULL;
new_slope->texRot = txt_angle;
// convert angle to vector
texture_orientation.x = (long)Cos(txt_angle) >> 6;
texture_orientation.y = (long)Sin(txt_angle) >> 6;
texture_orientation.z = 0;
// generate other endpoints from plane normal, texture origin, and texture
// orientation which determine the orientation of the texture's u v space
// in the 3d world's x, y, z space
// cross normal with texture orientation to get vector perpendicular to texture
// orientation and normal = v axis direction
V3Cross((Vector3D *)&(new_slope->plane), &texture_orientation, &v2);
// scale to size of texture in world space
V3Scale(&v2, FINENESS);
// cross normal with v axis direction vector to get vector perpendicular to v axis
// and normal = u axis direction vector
V3Cross(&v2, (Vector3D *)&(new_slope->plane), &v1);
// scale to size of texture in world space
V3Scale(&v1, FINENESS);
// add vectors to origin to get endpoints
V3Add(&new_slope->p0, &v1, &new_slope->p1);
V3Add(&new_slope->p0, &v2, &new_slope->p2);
// set flags indicating properties of the slope
new_slope->flags = 0;
if (ABS(new_slope->plane.c) < DIRECTIONAL_THRESHOLD)
new_slope->flags |= SLF_DIRECTIONAL;
else if (new_slope->plane.c < 0) // ceiling, apply same lighting hack as regular ceilings (see doDrawLeaf)
new_slope->lightscale = FINENESS-(shade_amount>>1);
else
