PerlinNoise::PerlinNoise(uint_t seed /* = 0 */) {
m_holdrand = seed;
int i, j, k;
for (i = 0 ; i < B ; i++) {
p[i] = i;
g1[i] = (float)((xrand() % (B + B)) - B) / B;
for (j = 0 ; j < 2 ; j++)
g2[i][j] = (float)((xrand() % (B + B)) - B) / B;
// normalize2(g2[i]);
for (j = 0 ; j < 3 ; j++)
g3[i][j] = (float)((xrand() % (B + B)) - B) / B;
normalize3(g3[i]);
}
while (--i) {
k = p[i];
p[i] = p[j = xrand() % B];
p[j] = k;
}
for (i = 0 ; i < B + 2 ; i++) {
p[B + i] = p[i];
g1[B + i] = g1[i];
for (j = 0 ; j < 2 ; j++)
g2[B + i][j] = g2[i][j];
for (j = 0 ; j < 3 ; j++)
g3[B + i][j] = g3[i][j];
}
}
void initNoise(void)
{
int i, j, k;
srand(30757);
for (i = 0 ; i < B ; i++) {
p[i] = i;
g1[i] = (double)((rand() % (B + B)) - B) / B;
for (j = 0 ; j < 2 ; j++)
g2[i][j] = (double)((rand() % (B + B)) - B) / B;
normalize2(g2[i]);
for (j = 0 ; j < 3 ; j++)
g3[i][j] = (double)((rand() % (B + B)) - B) / B;
normalize3(g3[i]);
}
while (--i) {
k = p[i];
p[i] = p[j = rand() % B];
p[j] = k;
}
for (i = 0 ; i < B + 2 ; i++) {
p[B + i] = p[i];
g1[B + i] = g1[i];
for (j = 0 ; j < 2 ; j++)
g2[B + i][j] = g2[i][j];
for (j = 0 ; j < 3 ; j++)
g3[B + i][j] = g3[i][j];
}
}
void CTerrain::init(void)
{
int i, j, k;
for (i = 0 ; i < BB ; i++) {
p[i] = i;
g1[i] = (float)((myRand() % (BB + BB)) - BB) / BB;
for (j = 0 ; j < 2 ; j++)
g2[i][j] = (float)((myRand() % (BB + BB)) - BB) / BB;
normalize2(g2[i]);
for (j = 0 ; j < 3 ; j++)
g3[i][j] = (float)((myRand() % (BB + BB)) - BB) / BB;
normalize3(g3[i]);
}
while (--i) {
k = p[i];
p[i] = p[j = myRand() % BB];
p[j] = k;
}
for (i = 0 ; i < BB + 2 ; i++) {
p[BB + i] = p[i];
g1[BB + i] = g1[i];
for (j = 0 ; j < 2 ; j++)
g2[BB + i][j] = g2[i][j];
for (j = 0 ; j < 3 ; j++)
g3[BB + i][j] = g3[i][j];
}
}
void Perlin::init(void)
{
int i, j, k; for (i = 0 ; i < mMaxSample; i++)
{
p[i] = i;
g1[i] = (double)((rand() % (mMaxSample + mMaxSample)) - mMaxSample) / mMaxSample;
for (j = 0 ; j < 2 ; j++)
g2[i][j] = (double)((rand() % (mMaxSample + mMaxSample)) - mMaxSample) / mMaxSample;
normalize2(g2[i]);
for (j = 0 ; j < 3 ; j++)
g3[i][j] = (double)((rand() % (mMaxSample + mMaxSample)) - mMaxSample) / mMaxSample;
normalize3(g3[i]);
}
while (--i)
{
k = p[i];
p[i] = p[j = rand() % mMaxSample];
p[j] = k;
}
for (i = 0 ; i < mMaxSample + 2 ; i++)
{
p[mMaxSample + i] = p[i];
g1[mMaxSample + i] = g1[i];
for (j = 0 ; j < 2 ; j++)
g2[mMaxSample + i][j] = g2[i][j];
for (j = 0 ; j < 3 ; j++)
g3[mMaxSample + i][j] = g3[i][j];
}
}
void noisetest::prepare()
{
int i, j, k;
for (i = 0 ; i < B ; i++) {
p[i] = i;
g1[i] = (double)((rand::ran(1) % (B + B)) - B) / B;
for (j = 0 ; j < 2 ; j++)
g2[i][j] = (double)((rand::ran(1) % (B + B)) - B) / B;
normalize2(g2[i]);
for (j = 0 ; j < 3 ; j++)
g3[i][j] = (double)((rand::ran(1) % (B + B)) - B) / B;
normalize3(g3[i]);
}
while (--i) {
k = p[i];
p[i] = p[j = rand::ran(1) % B];
p[j] = k;
}
for (i = 0 ; i < B + 2 ; i++) {
p[B + i] = p[i];
g1[B + i] = g1[i];
for (j = 0 ; j < 2 ; j++)
g2[B + i][j] = g2[i][j];
for (j = 0 ; j < 3 ; j++)
g3[B + i][j] = g3[i][j];
}
}
bool CalculatePolygonNormalFlat(float* position, float* buffer, int bufferLength, int elementSize, int polygonVertices)
{
for (int i = 0; i < bufferLength; i += (elementSize * polygonVertices))
{
float v1[3], v2[3], n[3], sum[] = {0.0f, 0.0f, 0.0f};
for (int j = 0; j < polygonVertices - 2; ++j)
{
float *p1 = &position[i],
*p2 = &position[i + (j + 2) * elementSize],
*p3 = &position[i + (j + 1) * elementSize];
MINUS3(p2, p1, v1);
MINUS3(p3, p1, v2);
CROSS3(v1, v2, n);
normalize3(n);
PLUS3(n, sum, sum);
}
for (int j = 0; j < polygonVertices; ++j)
{
COPY3(&buffer[i + j * elementSize], sum);
}
}
return true;
}
void init(void)
{
int i, j, k;
for (i = 0 ; i < BP ; i++) {
p[i] = i;
g1[i] = (double)((rand() % (BP + BP)) - BP) / BP;
for (j = 0 ; j < 2 ; j++)
g2[i][j] = (double)((rand() % (BP + BP)) - BP) / BP;
normalize2(g2[i]);
for (j = 0 ; j < 3 ; j++)
g3[i][j] = (double)((rand() % (BP + BP)) - BP) / BP;
normalize3(g3[i]);
}
while (--i) {
k = p[i];
p[i] = p[j = rand() % BP];
p[j] = k;
}
for (i = 0 ; i < BP + 2 ; i++) {
p[BP + i] = p[i];
g1[BP + i] = g1[i];
for (j = 0 ; j < 2 ; j++)
g2[BP + i][j] = g2[i][j];
for (j = 0 ; j < 3 ; j++)
g3[BP + i][j] = g3[i][j];
}
}
PerlinNoise::PerlinNoise(unsigned int seed)
{
int i, j, k;
//srand(seed);
for (i = 0 ; i < B ; i++) {
p[i] = i;
g1[i] = (float)((rand() % (B + B)) - B) / B;
for (j = 0 ; j < 2 ; j++)
g2[i][j] = (float)((rand() % (B + B)) - B) / B;
normalize2(g2[i]);
for (j = 0 ; j < 3 ; j++)
g3[i][j] = (float)((rand() % (B + B)) - B) / B;
normalize3(g3[i]);
}
while (--i) {
k = p[i];
p[i] = p[j = rand() % B];
p[j] = k;
}
for (i = 0 ; i < B + 2 ; i++) {
p[B + i] = p[i];
g1[B + i] = g1[i];
for (j = 0 ; j < 2 ; j++)
g2[B + i][j] = g2[i][j];
for (j = 0 ; j < 3 ; j++)
g3[B + i][j] = g3[i][j];
}
}
static void init(void)
{
int i, j, k;
for (i = 0 ; i < B ; i++) {
p[i] = i;
g1[i] = (float)((random() % (B + B)) - B) / B;
for (j = 0 ; j < 2 ; j++)
g2[i][j] = (float)((random() % (B + B)) - B) / B;
normalize2(g2[i]);
for (j = 0 ; j < 3 ; j++)
g3[i][j] = (float)((random() % (B + B)) - B) / B;
normalize3(g3[i]);
}
while (--i) {
k = p[i];
p[i] = p[j = random() % B];
p[j] = k;
}
for (i = 0 ; i < B + 2 ; i++) {
p[B + i] = p[i];
g1[B + i] = g1[i];
for (j = 0 ; j < 2 ; j++)
g2[B + i][j] = g2[i][j];
for (j = 0 ; j < 3 ; j++)
g3[B + i][j] = g3[i][j];
}
}
void PerlinGenerator::reset( int seed )
{
srand( seed ) ;
int i, j, k;
for (i = 0 ; i < perlin_B ; i++) {
p[i] = i;
g1[i] = (double)((rand() % (perlin_B + perlin_B)) - perlin_B) / perlin_B;
for (j = 0 ; j < 2 ; j++)
g2[i][j] = (double)((rand() % (perlin_B + perlin_B)) - perlin_B) / perlin_B;
normalize2(g2[i]);
for (j = 0 ; j < 3 ; j++)
g3[i][j] = (double)((rand() % (perlin_B + perlin_B)) - perlin_B) / perlin_B;
normalize3(g3[i]);
}
while (--i) {
k = p[i];
p[i] = p[j = rand() % perlin_B];
p[j] = k;
}
for (i = 0 ; i < perlin_B + 2 ; i++)
{
p[perlin_B + i] = p[i];
g1[perlin_B + i] = g1[i];
for (j = 0 ; j < 2 ; j++)
g2[perlin_B + i][j] = g2[i][j];
for (j = 0 ; j < 3 ; j++)
g3[perlin_B + i][j] = g3[i][j];
}
}
vec3 quaternion::axis() const
{
if( equal(w, 1.0f) ){
return vec3();
}
return normalize3(vec3(x, y, z));
}
quaternion log( const quaternion& lhs ){
float alpha = acos(lhs.w);
if( equal( std::abs(alpha), 1.0f ) ){
return lhs;
}
vec3 new_v = normalize3( lhs.comps().xyz() );
return quaternion(alpha*new_v[0], alpha*new_v[1], alpha*new_v[2], 0.0f);
}
void ClothSimulation::solve( float4* v, const Link& link )
{
float4 ab = v[link.m_b] - v[link.m_a];
float stretch = length3( ab ) - link.m_restLength;
stretch *= (1.f-link.m_dampingFactor);
float4 dx = stretch * normalize3( ab );
v[link.m_a] += dx*link.m_aWeight;
v[link.m_b] -= dx*link.m_bWeight;
}
void compute_normal(struct face *f)
{
/* TODO is this the right direction? I'm bad at rhr */
struct vertex a, b, n;
sub_vertex(&a, &f->v[0], &f->v[1]);
sub_vertex(&b, &f->v[0], &f->v[2]);
cross(&n, &a, &b);
normalize3(&n);
copy_vertex(&f->n[0], &n);
copy_vertex(&f->n[1], &n);
copy_vertex(&f->n[2], &n);
}
bool CalculateGridNormalFlat(float* position, float* buffer, int bufferLength, int elementSize)
{
if (elementSize < 3)
{
return false;
}
for (int i = 0; i < bufferLength; i += 4 * elementSize)
{
float *p1 = &position[i],
*p2 = &position[i + elementSize],
*p3 = &position[i + 2 * elementSize],
*p4 = &position[i + 3 * elementSize];
float v1[3], v2[3], n1[3], n2[3], n1pn2[3];
//v1 = p2-p1
MINUS3(p2, p1, v1);
MINUS3(p4, p1, v2);
//n1 = v1 x v2
CROSS3(v1, v2, n1);
MINUS3(p4, p1, v1);
MINUS3(p3, p1, v2);
CROSS3(v1, v2, n2);
normalize3(n1);
normalize3(n2);
PLUS3(n1, n2, n1pn2);
normalize3(n1pn2);
COPY3(&buffer[i], n1pn2);
COPY3(&buffer[i + elementSize], n1pn2);
COPY3(&buffer[i + 2 * elementSize], n1pn2);
COPY3(&buffer[i + 3 * elementSize], n1pn2);
}
return true;
}
quaternion exp( const quaternion& lhs ){
float alpha = lhs.norm();
vec3 v = normalize3( lhs.comps().xyz() );
float sin_alpha = 0.0f;
float cos_alpha = 0.0f;
sincos(alpha, sin_alpha, cos_alpha);
vec4 q_v;
q_v.w( cos_alpha );
q_v.xyz( sin_alpha * v );
return quaternion(q_v);
}
quaternion quaternion::from_axis_angle( const vec3& axis, float angle ){
vec3 normalized_axis = normalize3(axis);
float half_angle = angle / 2.0f;
float half_angle_sin = 0.0f;
float half_angle_cos = 0.0f;
sincos(half_angle, half_angle_sin, half_angle_cos);
return quaternion(
half_angle_sin * normalized_axis[0],
half_angle_sin * normalized_axis[1],
half_angle_sin * normalized_axis[2],
half_angle_cos
);
}
void
init (
int seed)
{
CLxPseudoRandom psr (seed);
int i, j, k;
for (i = 0 ; i < B ; i++)
{
p[i] = i;
g1[i] = psr.centered();
for (j = 0 ; j < 2 ; j++)
g2[i][j] = psr.centered();
normalize2(g2[i]);
for (j = 0 ; j < 3 ; j++)
g3[i][j] = psr.centered();
normalize3(g3[i]);
}
while (--i)
{
j = psr.range((int)B);
k = p[i];
p[i] = p[j];
p[j] = k;
}
for (i = 0 ; i < B + 2 ; i++)
{
p [B + i] = p[i];
g1[B + i] = g1[i];
for (j = 0 ; j < 2 ; j++)
g2[B + i][j] = g2[i][j];
for (j = 0 ; j < 3 ; j++)
g3[B + i][j] = g3[i][j];
}
}
void LLPerlinNoise::init(void)
{
int i, j, k;
for (i = 0 ; i < B ; i++)
{
p[i] = i;
g1[i] = (F32)((rand() % (B + B)) - B) / B;
for (j = 0 ; j < 2 ; j++)
g2[i][j] = (F32)((rand() % (B + B)) - B) / B;
normalize2(g2[i]);
for (j = 0 ; j < 3 ; j++)
g3[i][j] = (F32)((rand() % (B + B)) - B) / B;
normalize3(g3[i]);
}
while (--i)
{
k = p[i];
p[i] = p[j = rand() % B];
p[j] = k;
}
for (i = 0 ; i < B + 2 ; i++)
{
p[B + i] = p[i];
g1[B + i] = g1[i];
for (j = 0 ; j < 2 ; j++)
g2[B + i][j] = g2[i][j];
for (j = 0 ; j < 3 ; j++)
g3[B + i][j] = g3[i][j];
}
sInitialized = true;
}
Noise::Noise(long seed)
{
/* Use Blender RNG for repeatable results across platforms. */
RNG *rng = BLI_rng_new(seed);
int i, j, k;
for (i = 0; i < _NOISE_B; i++) {
p[i] = i;
g1[i] = (float)((BLI_rng_get_int(rng) % (_NOISE_B + _NOISE_B)) - _NOISE_B) / _NOISE_B;
for (j = 0; j < 2; j++)
g2[i][j] = (float)((BLI_rng_get_int(rng) % (_NOISE_B + _NOISE_B)) - _NOISE_B) / _NOISE_B;
normalize2(g2[i]);
for (j = 0; j < 3; j++)
g3[i][j] = (float)((BLI_rng_get_int(rng) % (_NOISE_B + _NOISE_B)) - _NOISE_B) / _NOISE_B;
normalize3(g3[i]);
}
while (--i) {
k = p[i];
p[i] = p[j = BLI_rng_get_int(rng) % _NOISE_B];
p[j] = k;
}
for (i = 0; i < _NOISE_B + 2; i++) {
p[_NOISE_B + i] = p[i];
g1[_NOISE_B + i] = g1[i];
for (j = 0; j < 2; j++)
g2[_NOISE_B + i][j] = g2[i][j];
for (j = 0; j < 3; j++)
g3[_NOISE_B + i][j] = g3[i][j];
}
BLI_rng_free(rng);
}
static void init(void)
{
int i, j, k;
// set the SEED
srand(1234);
for (i = 0 ; i < B ; i++) {
p[i] = i;
g1[i] = (double)((myRandomLI() % (B + B)) - B) / B;
for (j = 0 ; j < 2 ; j++)
g2[i][j] = (double)((myRandomLI() % (B + B)) - B) / B;
normalize2(g2[i]);
for (j = 0 ; j < 3 ; j++)
g3[i][j] = (double)((myRandomLI() % (B + B)) - B) / B;
normalize3(g3[i]);
}
while (--i) {
k = p[i];
p[i] = p[j = myRandomLI() % B];
p[j] = k;
}
for (i = 0 ; i < B + 2 ; i++) {
p[B + i] = p[i];
g1[B + i] = g1[i];
for (j = 0 ; j < 2 ; j++)
g2[B + i][j] = g2[i][j];
for (j = 0 ; j < 3 ; j++)
g3[B + i][j] = g3[i][j];
}
}
CoriolisEvaluate (Coriolis *inst, LWTextureAccess *ta)
{
/* Local stuff */
double rsq, angle, value, sine, cosine, turb;
/* Position Stuff */
double Pt[3], PtN[3], PP[3];
// Lets work in shader space
if (ta->axis == 0) {
Vec3Assign(Pt, ta->tPos[2], -ta->tPos[1], ta->tPos[0]);
} else if (ta->axis == 1) {
Vec3Assign(Pt, ta->tPos[0], -ta->tPos[2], ta->tPos[1]);
} else {
Vec3Assign(Pt, ta->tPos[0], -ta->tPos[1], ta->tPos[2]);
}
Vec3Copy(PtN, Pt);
normalize3(PtN);
rsq = xcomp(PtN) * xcomp(PtN) + ycomp(PtN) * ycomp(PtN);
angle = inst->tws[0] * rsq;
sine = sin( angle );
cosine = cos( angle );
PP[0] = Pt[0]*cosine - Pt[1]*sine;
PP[1] = Pt[0]*sine + Pt[1]*cosine;
PP[2] = Pt[2];
turb = fBm(PP, inst->inc[0], inst->lac[0], inst->oct[0], inst->fnoise);
value = Abs(inst->off[0] + inst->scl[0] * turb);
value = clamp(value, 0, 1);
return value;
}
static
__inline
void solveFriction(b3GpuConstraint4& cs,
const b3Vector3& posA, b3Vector3& linVelA, b3Vector3& angVelA, float invMassA, const b3Matrix3x3& invInertiaA,
const b3Vector3& posB, b3Vector3& linVelB, b3Vector3& angVelB, float invMassB, const b3Matrix3x3& invInertiaB,
float maxRambdaDt[4], float minRambdaDt[4])
{
if( cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0 ) return;
const b3Vector3& center = (const b3Vector3&)cs.m_center;
b3Vector3 n = -(const b3Vector3&)cs.m_linear;
b3Vector3 tangent[2];
#if 1
b3PlaneSpace1 (n, tangent[0],tangent[1]);
#else
b3Vector3 r = cs.m_worldPos[0]-center;
tangent[0] = cross3( n, r );
tangent[1] = cross3( tangent[0], n );
tangent[0] = normalize3( tangent[0] );
tangent[1] = normalize3( tangent[1] );
#endif
b3Vector3 angular0, angular1, linear;
b3Vector3 r0 = center - posA;
b3Vector3 r1 = center - posB;
for(int i=0; i<2; i++)
{
setLinearAndAngular( tangent[i], r0, r1, linear, angular0, angular1 );
float rambdaDt = calcRelVel(linear, -linear, angular0, angular1,
linVelA, angVelA, linVelB, angVelB );
rambdaDt *= cs.m_fJacCoeffInv[i];
{
float prevSum = cs.m_fAppliedRambdaDt[i];
float updated = prevSum;
updated += rambdaDt;
updated = b3Max( updated, minRambdaDt[i] );
updated = b3Min( updated, maxRambdaDt[i] );
rambdaDt = updated - prevSum;
cs.m_fAppliedRambdaDt[i] = updated;
}
b3Vector3 linImp0 = invMassA*linear*rambdaDt;
b3Vector3 linImp1 = invMassB*(-linear)*rambdaDt;
b3Vector3 angImp0 = (invInertiaA* angular0)*rambdaDt;
b3Vector3 angImp1 = (invInertiaB* angular1)*rambdaDt;
#ifdef _WIN32
b3Assert(_finite(linImp0.getX()));
b3Assert(_finite(linImp1.getX()));
#endif
linVelA += linImp0;
angVelA += angImp0;
linVelB += linImp1;
angVelB += angImp1;
}
{ // angular damping for point constraint
b3Vector3 ab = ( posB - posA ).normalized();
b3Vector3 ac = ( center - posA ).normalized();
if( b3Dot( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f))
{
float angNA = b3Dot( n, angVelA );
float angNB = b3Dot( n, angVelB );
angVelA -= (angNA*0.1f)*n;
angVelB -= (angNB*0.1f)*n;
}
}
}
void matrixApplyNormal(vec3* r, matrix a, vec3 b) {
r->x = a.v[0] * b.x + a.v[1] * b.y + a.v[2] * b.z;
r->y = a.v[4] * b.x + a.v[5] * b.y + a.v[6] * b.z;
r->z = a.v[8] * b.x + a.v[9] * b.y + a.v[10] * b.z;
normalize3( r );
}
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 SimpleDeferredDemo::render()
{
if( m_firstFrame )
{
for(int i=0; i<MAX_BODIES; i++)
{
const float4& pos = m_pos[i];
const Quaternion& quat = qtGetIdentity();
ShapeBase* boxShape = m_shapes[i];
const float4* vtx = boxShape->getVertexBuffer();
const int4* tris = boxShape->getTriangleBuffer();
float4* v = new float4[boxShape->getNumTris()*3];
u32* idx = new u32[boxShape->getNumTris()*3];
float4* n = new float4[boxShape->getNumTris()*3];
const float4 colors[] = { make_float4(0,1,1,1), make_float4(1,0,1,1), make_float4(1,1,0,1),
make_float4(0,0,1,1), make_float4(0,1,0,1), make_float4(1,0,0,1)
};
float c = 0.4f;
float4 color = make_float4(0.8f-c,0.8f-c,0.8f,1.f)*1.8f;
color = make_float4(0.5f,1,0.5f,1);
if( i==0 ) color = make_float4(1.f,1.f,1.f, 1.f);
for(int it=0; it<boxShape->getNumTris(); it++)
{
const int4& t = tris[it];
idx[3*it+0] = it*3;
idx[3*it+1] = it*3+1;
idx[3*it+2] = it*3+2;
v[3*it+0] = vtx[t.x];
v[3*it+1] = vtx[t.y];
v[3*it+2] = vtx[t.z];
if( i==0 ) swap2(v[3*it+1], v[3*it+2]);
float4 tn = cross3( v[3*it+1] - v[3*it+0], v[3*it+2] - v[3*it+0] );
tn = normalize3( tn );
n[3*it+0] = tn;
n[3*it+1] = tn;
n[3*it+2] = tn;
}
int nTris = boxShape->getNumTris();
int nVtx = boxShape->getNumVertex();
// pxReleaseDrawTriangleListTransformed( v, n, idx, nTris*3, nTris*3, color, pos, quat );
pxDrawTriangleListTransformed( v, n, idx, nTris*3, nTris*3, color, pos, quat );
delete [] v;
delete [] idx;
delete [] n;
}
// m_firstFrame = false;
}
}
int collideStraight(const ConvexHeightField* shapeA,const ConvexHeightField* shapeB,
const float4& bodyApos, Quaternion& bodyAquat,const float4& bodyBpos,const Quaternion& bodyBquat,
ContactPoint4* contactsOut, int& numContacts, int contactCapacity,
float collisionMargin )
{
// Stopwatch sw;
Transform trA;
trA = trSetTransform(bodyApos,bodyAquat);
Transform trB;
trB = trSetTransform(bodyBpos, bodyBquat);
Transform B2A;
{
Transform invTrA = trInvert( trA );
B2A = trMul( invTrA, trB );
}
int nContacts = 0;
{ // testB against A
float4 p[ConvexHeightField::HEIGHT_RES*ConvexHeightField::HEIGHT_RES*6];
int nHits = 0;
const float4* pInB = shapeB->getSamplePoints();
float4 baInB = qtInvRotate( bodyBquat, bodyApos - bodyBpos );
if( shapeA->m_type == CollisionShape::SHAPE_HEIGHT_FIELD )
baInB = make_float4(0,0,0,0);
// sw.start();
for(int iface=0; iface<6; iface++)
{
Aabb aabb = shapeB->m_faceAabbs[iface];
aabb.transform( B2A.m_translation, B2A.m_rotation );
if( !shapeA->m_aabb.overlaps( aabb ) ) continue;
for(int ip=0; ip<ConvexHeightField::HEIGHT_RES*ConvexHeightField::HEIGHT_RES; ip++)
{
int i = iface*ConvexHeightField::HEIGHT_RES*ConvexHeightField::HEIGHT_RES+ip;
if( dot3F4( baInB, pInB[i] ) < 0.f ) continue;
float4 pInA = trMul1( B2A, pInB[i] );
if( shapeA->m_aabb.overlaps( pInA ) )
{
// Stopwatch sw1;
// sw1.start();
float dist = shapeA->queryDistance( pInA );
// sw1.stop();
// m_times[TIME_SAMPLE] += sw1.getMs();
if( dist < collisionMargin )
{
p[nHits] = make_float4(pInA.x, pInA.y, pInA.z, dist);
nHits++;
}
}
}
}
// sw.stop();
// m_times[TIME_TEST] += sw.getMs();
// sw.start();
if( nHits )
{
float4 ab = bodyBpos - bodyApos;
ab = qtInvRotate( bodyAquat, ab );
if( shapeA->m_type == CollisionShape::SHAPE_HEIGHT_FIELD )
{
//todo. sample normal from height field but just fake here
ab = make_float4(0,1,0,0);
}
int cIdx[4];
float4 center;
nContacts = extractManifold( p, nHits, ab, center, cIdx );
float4 contactNormal;
{
shapeA->queryDistanceWithNormal( center, contactNormal );
contactNormal = normalize3( contactNormal );
// u32 cmp = u8vCompress( contactNormal );
// contactNormal = make_float4( u8vGetX(cmp), u8vGetY(cmp), u8vGetZ(cmp), 0 );
}
int writeIdx = atomAdd( &numContacts, 1 );
if( writeIdx+1 < contactCapacity )
{
ContactPoint4& c = contactsOut[writeIdx];
nContacts = min2( nContacts, 4 );
for(int i=0; i<nContacts; i++)
{
c.m_worldPos[i] = transform( p[cIdx[i]], bodyApos, bodyAquat );
c.m_worldPos[i].w = max2( p[cIdx[i]].w - collisionMargin, -2*collisionMargin );
}
c.m_worldNormal = normalize3( qtRotate( bodyAquat, contactNormal ) );
c.m_restituitionCoeff = 0.f;
c.m_frictionCoeff = 0.7f;
//c.m_bodyAPtr = (void*)bodyAIdx;
//c.m_bodyBPtr = (void*)bodyBIdx;
c.getNPoints() = nContacts;
}
}
// sw.stop();
// m_times[TIME_MANIFOLD] += sw.getMs();
}
return nContacts;
}
int extractManifold(const float4* p, int nPoints, float4& nearNormal, float4& centerOut,
int contactIdx[4])
{
if( nPoints == 0 ) return 0;
nPoints = min2( nPoints, 64 );
float4 center = make_float4(0.f);
{
float4 v[64];
memcpy( v, p, nPoints*sizeof(float4) );
PARALLEL_SUM( v, nPoints );
center = v[0]/(float)nPoints;
}
centerOut = center;
{ // sample 4 directions
if( nPoints < 4 )
{
for(int i=0; i<nPoints; i++) contactIdx[i] = i;
return nPoints;
}
float4 aVector = p[0] - center;
float4 u = cross3( nearNormal, aVector );
float4 v = cross3( nearNormal, u );
u = normalize3( u );
v = normalize3( v );
int idx[4];
float2 max00 = make_float2(0,FLT_MAX);
{
float4 dir0 = u;
float4 dir1 = -u;
float4 dir2 = v;
float4 dir3 = -v;
// idx, distance
{
{
int4 a[64];
for(int ie = 0; ie<nPoints; ie++ )
{
float4 f;
float4 r = p[ie]-center;
f.x = dot3F4( dir0, r );
f.y = dot3F4( dir1, r );
f.z = dot3F4( dir2, r );
f.w = dot3F4( dir3, r );
a[ie].x = ((*(u32*)&f.x) & 0xffffff00);
a[ie].x |= (0xff & ie);
a[ie].y = ((*(u32*)&f.y) & 0xffffff00);
a[ie].y |= (0xff & ie);
a[ie].z = ((*(u32*)&f.z) & 0xffffff00);
a[ie].z |= (0xff & ie);
a[ie].w = ((*(u32*)&f.w) & 0xffffff00);
a[ie].w |= (0xff & ie);
}
for(int ie=0; ie<nPoints; ie++)
{
a[0].x = (a[0].x > a[ie].x )? a[0].x: a[ie].x;
a[0].y = (a[0].y > a[ie].y )? a[0].y: a[ie].y;
a[0].z = (a[0].z > a[ie].z )? a[0].z: a[ie].z;
a[0].w = (a[0].w > a[ie].w )? a[0].w: a[ie].w;
}
idx[0] = (int)a[0].x & 0xff;
idx[1] = (int)a[0].y & 0xff;
idx[2] = (int)a[0].z & 0xff;
idx[3] = (int)a[0].w & 0xff;
}
}
{
float2 h[64];
PARALLEL_DO( h[ie] = make_float2((float)ie, p[ie].w), nPoints );
REDUCE_MIN( h, nPoints );
max00 = h[0];
}
}
contactIdx[0] = idx[0];
contactIdx[1] = idx[1];
contactIdx[2] = idx[2];
contactIdx[3] = idx[3];
// if( max00.y < 0.0f )
// contactIdx[0] = (int)max00.x;
std::sort( contactIdx, contactIdx+4 );
return 4;
}
}
static
__inline
void solveFriction(Constraint4& cs,
const float4& posA, float4& linVelA, float4& angVelA, float invMassA, const Matrix3x3& invInertiaA,
const float4& posB, float4& linVelB, float4& angVelB, float invMassB, const Matrix3x3& invInertiaB,
float maxRambdaDt[4], float minRambdaDt[4])
{
if( cs.m_fJacCoeffInv[0] == 0 && cs.m_fJacCoeffInv[0] == 0 ) return;
const float4& center = cs.m_center;
float4 n = -cs.m_linear;
float4 tangent[2];
#if 1
btPlaneSpace1 (&n, &tangent[0],&tangent[1]);
#else
float4 r = cs.m_worldPos[0]-center;
tangent[0] = cross3( n, r );
tangent[1] = cross3( tangent[0], n );
tangent[0] = normalize3( tangent[0] );
tangent[1] = normalize3( tangent[1] );
#endif
float4 angular0, angular1, linear;
float4 r0 = center - posA;
float4 r1 = center - posB;
for(int i=0; i<2; i++)
{
setLinearAndAngular( tangent[i], r0, r1, linear, angular0, angular1 );
float rambdaDt = calcRelVel(linear, -linear, angular0, angular1,
linVelA, angVelA, linVelB, angVelB );
rambdaDt *= cs.m_fJacCoeffInv[i];
{
float prevSum = cs.m_fAppliedRambdaDt[i];
float updated = prevSum;
updated += rambdaDt;
updated = max2( updated, minRambdaDt[i] );
updated = min2( updated, maxRambdaDt[i] );
rambdaDt = updated - prevSum;
cs.m_fAppliedRambdaDt[i] = updated;
}
float4 linImp0 = invMassA*linear*rambdaDt;
float4 linImp1 = invMassB*(-linear)*rambdaDt;
float4 angImp0 = mtMul1(invInertiaA, angular0)*rambdaDt;
float4 angImp1 = mtMul1(invInertiaB, angular1)*rambdaDt;
#ifdef _WIN32
btAssert(_finite(linImp0.x));
btAssert(_finite(linImp1.x));
#endif
linVelA += linImp0;
angVelA += angImp0;
linVelB += linImp1;
angVelB += angImp1;
}
{ // angular damping for point constraint
float4 ab = normalize3( posB - posA );
float4 ac = normalize3( center - posA );
if( dot3F4( ab, ac ) > 0.95f || (invMassA == 0.f || invMassB == 0.f))
{
float angNA = dot3F4( n, angVelA );
float angNB = dot3F4( n, angVelB );
angVelA -= (angNA*0.1f)*n;
angVelB -= (angNB*0.1f)*n;
}
}
}
bool CalculateGridNormalSmooth(float* position, float* buffer, int bufferLength, int elementSize, int numX, int numY)
{
if (elementSize < 3)
{
return false;
}
for (int i = 0; i < bufferLength; i += 4 * elementSize)
{
float *p1 = &position[i],
*p2 = &position[i + elementSize],
*p3 = &position[i + 2 * elementSize],
*p4 = &position[i + 3 * elementSize];
float v1[3], v2[3], n1[3], n2[3], n1pn2[3];
//v1 = p2-p1
MINUS3(p2, p1, v1);
MINUS3(p4, p1, v2);
//n1 = v1 x v2
CROSS3(v1, v2, n1);
MINUS3(p4, p1, v1);
MINUS3(p3, p1, v2);
CROSS3(v1, v2, n2);
normalize3(n1);
normalize3(n2);
PLUS3(n1, n2, n1pn2);
normalize3(n1pn2);
COPY3(&buffer[i], n1pn2);
COPY3(&buffer[i + elementSize], n1);
COPY3(&buffer[i + 2 * elementSize], n2);
COPY3(&buffer[i + 3 * elementSize], n1pn2);
}
// average normals in x axis
for (int i = 0; i < numX - 1; ++i)
{
for (int j = 0; j < numY - 2; ++j)
{
float sum[3];
float *p2, *p3, *pj0, *pj1;
p2 = getGridNormal(buffer, numX, numY, elementSize, i, j, 2);
p3 = getGridNormal(buffer, numX, numY, elementSize, i, j, 3);
pj0 = getGridNormal(buffer, numX, numY, elementSize, i, j + 1, 0);
pj1 = getGridNormal(buffer, numX, numY, elementSize, i, j + 1, 1);
PLUS3(pj0, p2, sum);
COPY3(p2, sum);
COPY3(pj0, sum);
PLUS3(pj1, p3, sum);
COPY3(p3, sum);
COPY3(pj1, sum);
}
}
// average normals in y axis
for (int j = 0; j < numY - 1; ++j)
{
for (int i = 0; i < numX - 2; ++i)
{
float sum[3];
float *p1, *p3, *pi0, *pi2;
p1 = getGridNormal(buffer, numX, numY, elementSize, i, j, 1);
p3 = getGridNormal(buffer, numX, numY, elementSize, i, j, 3);
pi0 = getGridNormal(buffer, numX, numY, elementSize, i + 1, j, 0);
pi2 = getGridNormal(buffer, numX, numY, elementSize, i + 1, j, 2);
PLUS3(pi0, p1, sum);
COPY3(p1, sum);
COPY3(pi0, sum);
PLUS3(pi2, p3, sum);
COPY3(p3, sum);
COPY3(pi2, sum);
}
}
return true;
}
