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; }