static
__forceinline
void setLinearAndAngular(const float4& n, const float4& r0, const float4& r1,
float4& linear, float4& angular0, float4& angular1)
{
linear = -n;
angular0 = -cross3(r0, n);
angular1 = cross3(r1, n);
}
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;
}
void inertia_triangle(double *v0, double *v1, double *v2,
double mass, double *inertia)
{
double s[3][3] = {{2.0, 1.0, 1.0}, {1.0, 2.0, 1.0}, {1.0, 1.0, 2.0}};
double v[3][3],sv[3][3],vtsv[3][3];
double vvv[3],v1mv0[3],v2mv0[3],normal[3];
v[0][0] = v0[0]; v[0][1] = v0[1]; v[0][2] = v0[2];
v[1][0] = v1[0]; v[1][1] = v1[1]; v[1][2] = v1[2];
v[2][0] = v2[0]; v[2][1] = v2[1]; v[2][2] = v2[2];
times3(s,v,sv);
transpose_times3(v,sv,vtsv);
double sum = lensq3(v0) + lensq3(v1) + lensq3(v2);
vvv[0] = v0[0] + v1[0] + v2[0];
vvv[1] = v0[1] + v1[1] + v2[1];
vvv[2] = v0[2] + v1[2] + v2[2];
sum += lensq3(vvv);
sub3(v1,v0,v1mv0);
sub3(v2,v0,v2mv0);
cross3(v1mv0,v2mv0,normal);
double a = len3(normal);
double inv24 = mass/24.0;
inertia[0] = inv24*a*(sum-vtsv[0][0]);
inertia[1] = inv24*a*(sum-vtsv[1][1]);
inertia[2] = inv24*a*(sum-vtsv[2][2]);
inertia[3] = -inv24*a*vtsv[1][2];
inertia[4] = -inv24*a*vtsv[0][2];
inertia[5] = -inv24*a*vtsv[0][1];
}
void calcRigidMotion(const float4* vtx, const float4* vtxPrev, int nVtx, float4& v, float4& w, float4& c)
{
float4 ang;
Matrix3x3 inertia;
c = make_float4(0);
v = make_float4(0);
ang = make_float4(0);
inertia = mtIdentity();
for(int i=0; i<nVtx; i++)
{
c += vtx[i];
v += vtx[i]-vtxPrev[i];
}
c /= (float)nVtx;
v /= (float)nVtx;
for(int i=0; i<nVtx; i++)
{
const float4& r = vtx[i]-c;
ang += cross3( r, vtx[i]-vtxPrev[i] );
Matrix3x3 m;
m.m_row[0] = make_float4(r.y*r.y + r.z*r.z, -r.x*r.y, -r.x*r.z);
m.m_row[1] = make_float4(-r.y*r.x, r.x*r.x + r.z*r.z, -r.y*r.z);
m.m_row[2] = make_float4(-r.z*r.x, -r.z*r.y, r.x*r.x + r.y*r.y);
inertia = inertia + m;
}
Matrix3x3 invInertia = mtInvert( inertia );
w = mtMul1( invInertia, ang );
}
//-----------------------------------------------------------------------------
void CPUTCamera::LookAt( const float xx, const float yy, const float zz )
{
float3 pos;
GetPosition( &pos);
float3 lookPoint(xx, yy, zz);
float3 look = (lookPoint - pos).normalize();
float3 right = cross3(float3(0.0f,1.0f,0.0f), look).normalize(); // TODO: simplify algebraically
float3 up = cross3(look, right);
mParentMatrix = float4x4(
right.x, right.y, right.z, 0.0f,
up.x, up.y, up.z, 0.0f,
look.x, look.y, look.z, 0.0f,
pos.x, pos.y, pos.z, 1.0f
);
}
float ClothSimulation::calcVolume( const float4* v, const int4* t, int n )
{
float vol = 0;
for(int i=0; i<n; i++)
{
const int4& tri = t[i];
float4 c = cross3(v[tri.y], v[tri.x]);
vol += dot3F4( c, v[tri.z] );
}
return fabs( vol/6.f );
}
static int luaB_cross (lua_State *L) {
float x1, y1, z1;
float x2, y2, z2;
float ax, ay, az;
if (lua_gettop(L) != 2) luaL_error(L, "Invalid params, try cross(v,v)");
lua_checkvector3(L, 1, &x1, &y1, &z1);
lua_checkvector3(L, 2, &x2, &y2, &z2);
cross3(x1,y1,z1, x2,y2,z2, &ax, &ay, &az);
lua_pushvector3(L,ax,ay,az);
return 1;
}
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);
}
void lookat(vec3 eye, vec3 center, vec3 up, float* camera) {
vec3 z = normal3(sub3(eye, center));
vec3 x = normal3(cross3(up,z));
vec3 y = normal3(cross3(z,x));
float Minv[4][4];
float Tr[4][4];
mat_identity(&Minv[0][0]);
mat_identity(&Tr[0][0]);
Minv[0][0] = x.x;
Minv[1][0] = y.x;
Minv[2][0] = z.x;
Tr[0][3] = -center.x;
Minv[0][1] = x.y;
Minv[1][1] = y.y;
Minv[2][1] = z.y;
Tr[1][3] = -center.y;
Minv[0][2] = x.z;
Minv[1][2] = y.z;
Minv[2][2] = z.z;
Tr[2][3] = -center.z;
matmul(&Minv[0][0], &Tr[0][0], camera);
}
/* satellite antenna phase center offset ---------------------------------------
* compute satellite antenna phase center offset in ecef
* args : gtime_t time I time (gpst)
* double *rs I satellite position and velocity (ecef)
* {x,y,z,vx,vy,vz} (m|m/s)
* pcv_t *pcv I satellite antenna parameter
* double *dant I satellite antenna phase center offset (ecef)
* {dx,dy,dz} (m)
* return : none
*-----------------------------------------------------------------------------*/
extern void satantoff(gtime_t time, const double *rs, const pcv_t *pcv,
double *dant)
{
double ex[3],ey[3],ez[3],es[3],r[3],rsun[3],gmst;
int i;
trace(4,"satantoff: time=%s\n",time_str(time,3));
/* sun position in ecef */
sunmoonpos(gpst2utc(time),NULL,rsun,NULL,&gmst);
/* unit vectors of satellite fixed coordinates */
for (i=0;i<3;i++) r[i]=-rs[i];
if (!normv3(r,ez)) return;
for (i=0;i<3;i++) r[i]=rsun[i]-rs[i];
if (!normv3(r,es)) return;
cross3(ez,es,r);
if (!normv3(r,ey)) return;
cross3(ey,ez,ex);
for (i=0;i<3;i++) { /* use L1 value */
dant[i]=pcv->off[0][0]*ex[i]+pcv->off[0][1]*ey[i]+pcv->off[0][2]*ez[i];
}
}
void ClothSimulation::volumeConstraint( float4* vtx, float* mass, const int4* tris, int nTris, float initVolume, float coeff )
{
float vol = calcVolume( vtx, tris, nTris );
float force = (vol - initVolume)*coeff;
int nVtx = 0;
for(int i=0; i<nTris; i++)
{
nVtx = max2( nVtx, max2( tris[i].x, max2( tris[i].y, tris[i].z) ) );
}
nVtx += 1;
float4* disp = new float4[nVtx];
for(int i=0; i<nVtx; i++) disp[i] = make_float4(0,0,0,0);
for(int i=0; i<nTris; i++)
{
const int4& t = tris[i];
float4& v0 = vtx[t.x];
float4& v1 = vtx[t.y];
float4& v2 = vtx[t.z];
const float4 n = cross3(v1-v0, v2-v0 );
float nl = length3( n );
float area = nl/2.f;
// force = max2( 0.f, force );
force = min2( 0.f, force );
float4 f = force/3.f*(n/nl);
if( mass[t.x] != FLT_MAX )
disp[t.x] += f;
// v0 += f;
if( mass[t.y] != FLT_MAX )
disp[t.y] += f;
// v1 += f;
if( mass[t.z] != FLT_MAX )
disp[t.z] += f;
// v2 += f;
}
for(int i=0; i<nVtx; i++)
vtx[i] += disp[i];
delete [] disp;
}
void
MeshFaceElementTable::calcNormalVector(int index)
{
if (normals == NULL)
return;
meshElementCode elem_code = getElementCode(index);
// Pick normal
Point3& normal = normals[index];
Point3 *p1, *p2, *p3;
switch (elem_code) {
case MEC_303:
case MEC_304:
case MEC_306:
case MEC_307:
p1 = &meshNodes[nodeIds[index][0]];
p2 = &meshNodes[nodeIds[index][1]];
p3 = &meshNodes[nodeIds[index][2]];
break;
case MEC_404:
case MEC_405:
case MEC_408:
case MEC_409:
p1 = &meshNodes[nodeIds[index][0]];
p2 = &meshNodes[nodeIds[index][1]];
p3 = &meshNodes[nodeIds[index][2]];
break;
default:
return;
}
Point3 a, b;
diff3(*p3, *p1, a);
diff3(*p2, *p1, b);
cross3(b, a, normal);
normalize(normal);
for (int i = 0; i < 3; i++) {
if ( isZero(normal[i]) ) {
normal[i] = 0.0;
}
}
}
/*
* multiply the top of the matrix stack by a view-pointing transformation
* with the eyepoint at e, looking at point l, with u at the top of the screen.
* The coordinate system is deemed to be right-handed.
* The generated transformation transforms this view into a view from
* the origin, looking in the positive y direction, with the z axis pointing up,
* and x to the right.
*/
void look(Space *t, Point3 e, Point3 l, Point3 u){
Matrix m, inv;
Point3 r;
l=unit3(sub3(l, e));
u=unit3(vrem3(sub3(u, e), l));
r=cross3(l, u);
/* make the matrix to transform from (rlu) space to (xyz) space */
ident(m);
m[0][0]=r.x; m[0][1]=r.y; m[0][2]=r.z;
m[1][0]=l.x; m[1][1]=l.y; m[1][2]=l.z;
m[2][0]=u.x; m[2][1]=u.y; m[2][2]=u.z;
ident(inv);
inv[0][0]=r.x; inv[0][1]=l.x; inv[0][2]=u.x;
inv[1][0]=r.y; inv[1][1]=l.y; inv[1][2]=u.y;
inv[2][0]=r.z; inv[2][1]=l.z; inv[2][2]=u.z;
ixform(t, m, inv);
move(t, -e.x, -e.y, -e.z);
}
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;
}
}
Vector4 Vector4::anotherPerpendicular3(const Vector3& hint, const Vector3& hint2) const
{
Vector4 firstPerp = perpendicular3(hint, hint2);
Vector4 v = cross3(firstPerp);
return v.normalized3();
}
/* satellite position and clock with ssr correction --------------------------*/
static int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
int opt, double *rs, double *dts, double *var, int *svh)
{
const ssr_t *ssr;
eph_t *eph;
double t1,t2,t3,er[3],ea[3],ec[3],rc[3],deph[3],dclk,dant[3]={0},tk;
int i,sys;
trace(4,"satpos_ssr: time=%s sat=%2d\n",time_str(time,3),sat);
ssr=nav->ssr+sat-1;
if (!ssr->t0[0].time) {
trace(2,"no ssr orbit correction: %s sat=%2d\n",time_str(time,0),sat);
return 0;
}
if (!ssr->t0[1].time) {
trace(2,"no ssr clock correction: %s sat=%2d\n",time_str(time,0),sat);
return 0;
}
/* inconsistency between orbit and clock correction */
if (ssr->iod[0]!=ssr->iod[1]) {
trace(2,"inconsist ssr correction: %s sat=%2d iod=%d %d\n",
time_str(time,0),sat,ssr->iod[0],ssr->iod[1]);
*svh=-1;
return 0;
}
t1=timediff(time,ssr->t0[0]);
t2=timediff(time,ssr->t0[1]);
t3=timediff(time,ssr->t0[2]);
/* ssr orbit and clock correction (ref [4]) */
if (fabs(t1)>MAXAGESSR||fabs(t2)>MAXAGESSR) {
trace(2,"age of ssr error: %s sat=%2d t=%.0f %.0f\n",time_str(time,0),
sat,t1,t2);
*svh=-1;
return 0;
}
if (ssr->udi[0]>=1.0) t1-=ssr->udi[0]/2.0;
if (ssr->udi[1]>=1.0) t2-=ssr->udi[0]/2.0;
for (i=0;i<3;i++) deph[i]=ssr->deph[i]+ssr->ddeph[i]*t1;
dclk=ssr->dclk[0]+ssr->dclk[1]*t2+ssr->dclk[2]*t2*t2;
/* ssr highrate clock correction (ref [4]) */
if (ssr->iod[0]==ssr->iod[2]&&ssr->t0[2].time&&fabs(t3)<MAXAGESSR_HRCLK) {
dclk+=ssr->hrclk;
}
if (norm(deph,3)>MAXECORSSR||fabs(dclk)>MAXCCORSSR) {
trace(3,"invalid ssr correction: %s deph=%.1f dclk=%.1f\n",
time_str(time,0),norm(deph,3),dclk);
*svh=-1;
return 0;
}
/* satellite postion and clock by broadcast ephemeris */
if (!ephpos(time,teph,sat,nav,ssr->iode,rs,dts,var,svh)) return 0;
/* satellite clock for gps, galileo and qzss */
sys=satsys(sat,NULL);
if (sys==SYS_GPS||sys==SYS_GAL||sys==SYS_QZS||sys==SYS_CMP) {
if (!(eph=seleph(teph,sat,ssr->iode,nav))) return 0;
/* satellite clock by clock parameters */
tk=timediff(time,eph->toc);
dts[0]=eph->f0+eph->f1*tk+eph->f2*tk*tk;
dts[1]=eph->f1+2.0*eph->f2*tk;
/* relativity correction */
dts[0]-=2.0*dot(rs,rs+3,3)/CLIGHT/CLIGHT;
}
/* radial-along-cross directions in ecef */
if (!normv3(rs+3,ea)) return 0;
cross3(rs,rs+3,rc);
if (!normv3(rc,ec)) {
*svh=-1;
return 0;
}
cross3(ea,ec,er);
/* satellite antenna offset correction */
if (opt) {
satantoff(time,rs,sat,nav,dant);
}
for (i=0;i<3;i++) {
rs[i]+=-(er[i]*deph[0]+ea[i]*deph[1]+ec[i]*deph[2])+dant[i];
}
/* t_corr = t_sv - (dts(brdc) + dclk(ssr) / CLIGHT) (ref [10] eq.3.12-7) */
dts[0]+=dclk/CLIGHT;
/* variance by ssr ura */
*var=var_urassr(ssr->ura);
trace(5,"satpos_ssr: %s sat=%2d deph=%6.3f %6.3f %6.3f er=%6.3f %6.3f %6.3f dclk=%6.3f var=%6.3f\n",
time_str(time,2),sat,deph[0],deph[1],deph[2],er[0],er[1],er[2],dclk,*var);
return 1;
}
//-----------------------------------------------------------------------------
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
}
}
}
}
}
//-----------------------------------------------
void CPUTFrustum::InitializeFrustum
(
float nearClipDistance,
float farClipDistance,
float aspectRatio,
float fov,
const float3 &position,
const float3 &look,
const float3 &up
)
{
// ******************************
// This function computes the position of each of the frustum's eight points.
// It also computes the normal of each of the frustum's six planes.
// ******************************
mNumFrustumVisibleModels = 0;
mNumFrustumCulledModels = 0;
// We have the camera's up and look, but we also need right.
float3 right = cross3( up, look );
// Compute the position of the center of the near and far clip planes.
float3 nearCenter = position + look * nearClipDistance;
float3 farCenter = position + look * farClipDistance;
// Compute the width and height of the near and far clip planes
float tanHalfFov = gScale2 * tanf(0.5f*fov);
float halfNearWidth = nearClipDistance * tanHalfFov;
float halfNearHeight = halfNearWidth / aspectRatio;
float halfFarWidth = farClipDistance * tanHalfFov;
float halfFarHeight = halfFarWidth / aspectRatio;
// Create two vectors each for the near and far clip planes.
// These are the scaled up and right vectors.
float3 upNear = up * halfNearHeight;
float3 rightNear = right * halfNearWidth;
float3 upFar = up * halfFarHeight;
float3 rightFar = right * halfFarWidth;
// Use the center positions and the up and right vectors
// to compute the positions for the near and far clip plane vertices (four each)
mpPosition[0] = nearCenter + upNear - rightNear; // near top left
mpPosition[1] = nearCenter + upNear + rightNear; // near top right
mpPosition[2] = nearCenter - upNear + rightNear; // near bottom right
mpPosition[3] = nearCenter - upNear - rightNear; // near bottom left
mpPosition[4] = farCenter + upFar - rightFar; // far top left
mpPosition[5] = farCenter + upFar + rightFar; // far top right
mpPosition[6] = farCenter - upFar + rightFar; // far bottom right
mpPosition[7] = farCenter - upFar - rightFar; // far bottom left
// Compute some of the frustum's edge vectors. We will cross these
// to get the normals for each of the six planes.
float3 nearTop = mpPosition[1] - mpPosition[0];
float3 nearLeft = mpPosition[3] - mpPosition[0];
float3 topLeft = mpPosition[4] - mpPosition[0];
float3 bottomRight = mpPosition[2] - mpPosition[6];
float3 farRight = mpPosition[5] - mpPosition[6];
float3 farBottom = mpPosition[7] - mpPosition[6];
mpNormal[0] = cross3(nearTop, nearLeft).normalize(); // near clip plane
mpNormal[1] = cross3(nearLeft, topLeft).normalize(); // left
mpNormal[2] = cross3(topLeft, nearTop).normalize(); // top
mpNormal[3] = cross3(farBottom, bottomRight).normalize(); // bottom
mpNormal[4] = cross3(bottomRight, farRight).normalize(); // right
mpNormal[5] = cross3(farRight, farBottom).normalize(); // far clip plane
}
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;
}
}
void basic_gear()
{
char rcsid[] = "junk";
#define NUM_WHEELS 4
// char gear_strings[NUM_WHEELS][12]={"nose","right main", "left main", "tail skid"};
/*
* Aircraft specific initializations and data goes here
*/
static int num_wheels = NUM_WHEELS; /* number of wheels */
static DATA d_wheel_rp_body_v[NUM_WHEELS][3] = /* X, Y, Z locations,full extension */
{
{ .422, 0., .29 }, /*nose*/ /* in feet */
{ 0.026, 0.006, .409 }, /*right main*/
{ 0.026, -.006, .409 }, /*left main*/
{ -1.32, 0, .17 } /*tail skid */
};
// static DATA gear_travel[NUM_WHEELS] = /*in Z-axis*/
// { -0.5, 2.5, 2.5, 0};
static DATA spring_constant[NUM_WHEELS] = /* springiness, lbs/ft */
{ 2., .65, .65, 1. };
static DATA spring_damping[NUM_WHEELS] = /* damping, lbs/ft/sec */
{ 1., .3, .3, .5 };
static DATA percent_brake[NUM_WHEELS] = /* percent applied braking */
{ 0., 0., 0., 0. }; /* 0 = none, 1 = full */
static DATA caster_angle_rad[NUM_WHEELS] = /* steerable tires - in */
{ 0., 0., 0., 0}; /* radians, +CW */
/*
* End of aircraft specific code
*/
/*
* Constants & coefficients for tyres on tarmac - ref [1]
*/
/* skid function looks like:
*
* mu ^
* |
* max_mu | +
* | /|
* sliding_mu | / +------
* | /
* | /
* +--+------------------------>
* | | | sideward V
* 0 bkout skid
* V V
*/
static int it_rolls[NUM_WHEELS] = { 1,1,1,0};
static DATA sliding_mu[NUM_WHEELS] = { 0.5, 0.5, 0.5, 0.3};
static DATA rolling_mu[NUM_WHEELS] = { 0.01, 0.01, 0.01, 0.0};
static DATA max_brake_mu[NUM_WHEELS] ={ 0.0, 0.6, 0.6, 0.0};
static DATA max_mu = 0.8;
static DATA bkout_v = 0.1;
static DATA skid_v = 1.0;
/*
* Local data variables
*/
DATA d_wheel_cg_body_v[3]; /* wheel offset from cg, X-Y-Z */
DATA d_wheel_cg_local_v[3]; /* wheel offset from cg, N-E-D */
DATA d_wheel_rwy_local_v[3]; /* wheel offset from rwy, N-E-U */
DATA v_wheel_cg_local_v[3]; /*wheel velocity rel to cg N-E-D*/
// DATA v_wheel_body_v[3]; /* wheel velocity, X-Y-Z */
DATA v_wheel_local_v[3]; /* wheel velocity, N-E-D */
DATA f_wheel_local_v[3]; /* wheel reaction force, N-E-D */
// DATA altitude_local_v[3]; /*altitude vector in local (N-E-D) i.e. (0,0,h)*/
// DATA altitude_body_v[3]; /*altitude vector in body (X,Y,Z)*/
DATA temp3a[3];
// DATA temp3b[3];
DATA tempF[3];
DATA tempM[3];
DATA reaction_normal_force; /* wheel normal (to rwy) force */
DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle; /* temp storage */
DATA v_wheel_forward, v_wheel_sideward, abs_v_wheel_sideward;
DATA forward_mu, sideward_mu; /* friction coefficients */
DATA beta_mu; /* breakout friction slope */
DATA forward_wheel_force, sideward_wheel_force;
int i; /* per wheel loop counter */
/*
* Execution starts here
*/
beta_mu = max_mu/(skid_v-bkout_v);
clear3( F_gear_v ); /* Initialize sum of forces... */
clear3( M_gear_v ); /* ...and moments */
/*
* Put aircraft specific executable code here
*/
percent_brake[1] = Brake_pct[0];
percent_brake[2] = Brake_pct[1];
caster_angle_rad[0] =
(0.01 + 0.04 * (1 - V_calibrated_kts / 130)) * Rudder_pedal;
for (i=0;i<num_wheels;i++) /* Loop for each wheel */
{
/* printf("%s:\n",gear_strings[i]); */
/*========================================*/
/* Calculate wheel position w.r.t. runway */
/*========================================*/
/* printf("\thgcg: %g, theta: %g,phi: %g\n",D_cg_above_rwy,Theta*RAD_TO_DEG,Phi*RAD_TO_DEG); */
/* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
sub3( d_wheel_rp_body_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
/* then converting to local (North-East-Down) axes... */
multtrans3x3by3( T_local_to_body_m, d_wheel_cg_body_v, d_wheel_cg_local_v );
/* Runway axes correction - third element is Altitude, not (-)Z... */
d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
/* Add wheel offset to cg location in local axes */
add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
/* remove Runway axes correction so right hand rule applies */
d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
/*============================*/
/* Calculate wheel velocities */
/*============================*/
/* contribution due to angular rates */
cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
/* transform into local axes */
multtrans3x3by3( T_local_to_body_m, temp3a,v_wheel_cg_local_v );
/* plus contribution due to cg velocities */
add3( v_wheel_cg_local_v, V_local_rel_ground_v, v_wheel_local_v );
clear3(f_wheel_local_v);
reaction_normal_force=0;
if( HEIGHT_AGL_WHEEL < 0. )
/*the wheel is underground -- which implies ground contact
so calculate reaction forces */
{
/*===========================================*/
/* Calculate forces & moments for this wheel */
/*===========================================*/
/* Add any anticipation, or frame lead/prediction, here... */
/* no lead used at present */
/* Calculate sideward and forward velocities of the wheel
in the runway plane */
cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
v_wheel_forward = v_wheel_local_v[0]*cos_wheel_hdg_angle
+ v_wheel_local_v[1]*sin_wheel_hdg_angle;
v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
- v_wheel_local_v[0]*sin_wheel_hdg_angle;
/* Calculate normal load force (simple spring constant) */
reaction_normal_force = 0.;
reaction_normal_force = spring_constant[i]*d_wheel_rwy_local_v[2]
- v_wheel_local_v[2]*spring_damping[i];
/* printf("\treaction_normal_force: %g\n",reaction_normal_force); */
if (reaction_normal_force > 0.) reaction_normal_force = 0.;
/* to prevent damping component from swamping spring component */
/* Calculate friction coefficients */
if(it_rolls[i])
{
forward_mu = (max_brake_mu[i] - rolling_mu[i])*percent_brake[i] + rolling_mu[i];
abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
sideward_mu = sliding_mu[i];
if (abs_v_wheel_sideward < skid_v)
sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
}
else
{
forward_mu=sliding_mu[i];
sideward_mu=sliding_mu[i];
}
/* Calculate foreward and sideward reaction forces */
forward_wheel_force = forward_mu*reaction_normal_force;
sideward_wheel_force = sideward_mu*reaction_normal_force;
if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
/* printf("\tFfwdgear: %g Fsidegear: %g\n",forward_wheel_force,sideward_wheel_force);
*/
/* Rotate into local (N-E-D) axes */
f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
- sideward_wheel_force*sin_wheel_hdg_angle;
f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
+ sideward_wheel_force*cos_wheel_hdg_angle;
f_wheel_local_v[2] = reaction_normal_force;
/* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
/* Calculate moments from force and offsets in body axes */
cross3( d_wheel_cg_body_v, tempF, tempM );
/* Sum forces and moments across all wheels */
add3( tempF, F_gear_v, F_gear_v );
add3( tempM, M_gear_v, M_gear_v );
}
/* printf("\tN: %g,dZrwy: %g dZdotrwy: %g\n",reaction_normal_force,HEIGHT_AGL_WHEEL,v_wheel_cg_local_v[2]); */
/* printf("\tFxgear: %g Fygear: %g, Fzgear: %g\n",F_X_gear,F_Y_gear,F_Z_gear); */
/* printf("\tMgear: %g, Lgear: %g, Ngear: %g\n\n",M_m_gear,M_l_gear,M_n_gear); */
}
}
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;
}
}
}
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;
}
}
void uiuc_gear()
{
/*
* Aircraft specific initializations and data goes here
*/
static DATA percent_brake[MAX_GEAR] = /* percent applied braking */
{ 0., 0., 0., 0.,
0., 0., 0., 0.,
0., 0., 0., 0.,
0., 0., 0., 0.
}; /* 0 = none, 1 = full */
static DATA caster_angle_rad[MAX_GEAR] = /* steerable tires - in */
{ 0., 0., 0., 0.,
0., 0., 0., 0.,
0., 0., 0., 0.,
0., 0., 0., 0.
}; /* radians, +CW */
/*
* End of aircraft specific code
*/
/*
* Constants & coefficients for tyres on tarmac - ref [1]
*/
/* skid function looks like:
*
* mu ^
* |
* max_mu | +
* | /|
* sliding_mu | / +------
* | /
* | /
* +--+------------------------>
* | | | sideward V
* 0 bkout skid
* V V
*/
static int it_rolls[MAX_GEAR] =
{ 1, 1, 1, 0,
0, 0, 0, 0,
0, 0, 0, 0,
0, 0, 0, 0
};
static DATA sliding_mu[MAX_GEAR] =
{ 0.5, 0.5, 0.5, 0.3,
0.3, 0.3, 0.3, 0.3,
0.3, 0.3, 0.3, 0.3,
0.3, 0.3, 0.3, 0.3
};
static DATA max_brake_mu[MAX_GEAR] =
{ 0.0, 0.6, 0.6, 0.0,
0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0
};
static DATA max_mu = 0.8;
static DATA bkout_v = 0.1;
static DATA skid_v = 1.0;
/*
* Local data variables
*/
DATA d_wheel_cg_body_v[3]; /* wheel offset from cg, X-Y-Z */
DATA d_wheel_cg_local_v[3]; /* wheel offset from cg, N-E-D */
DATA d_wheel_rwy_local_v[3]; /* wheel offset from rwy, N-E-U */
DATA v_wheel_cg_local_v[3]; /*wheel velocity rel to cg N-E-D*/
// DATA v_wheel_body_v[3]; /* wheel velocity, X-Y-Z */
DATA v_wheel_local_v[3]; /* wheel velocity, N-E-D */
DATA f_wheel_local_v[3]; /* wheel reaction force, N-E-D */
// DATA altitude_local_v[3]; /*altitude vector in local (N-E-D) i.e. (0,0,h)*/
// DATA altitude_body_v[3]; /*altitude vector in body (X,Y,Z)*/
DATA temp3a[3];
// DATA temp3b[3];
DATA tempF[3];
DATA tempM[3];
DATA reaction_normal_force; /* wheel normal (to rwy) force */
DATA cos_wheel_hdg_angle, sin_wheel_hdg_angle; /* temp storage */
DATA v_wheel_forward, v_wheel_sideward, abs_v_wheel_sideward;
DATA forward_mu, sideward_mu; /* friction coefficients */
DATA beta_mu; /* breakout friction slope */
DATA forward_wheel_force, sideward_wheel_force;
int i; /* per wheel loop counter */
/*
* Execution starts here
*/
beta_mu = max_mu/(skid_v-bkout_v);
clear3( F_gear_v ); /* Initialize sum of forces... */
clear3( M_gear_v ); /* ...and moments */
/*
* Put aircraft specific executable code here
*/
percent_brake[1] = Brake_pct[0];
percent_brake[2] = Brake_pct[1];
caster_angle_rad[0] =
(0.01 + 0.04 * (1 - V_calibrated_kts / 130)) * Rudder_pedal;
for (i=0; i<MAX_GEAR; i++) /* Loop for each wheel */
{
// Execute only if the gear has been defined
if (!gear_model[i])
{
// do nothing
continue;
}
else
{
/*========================================*/
/* Calculate wheel position w.r.t. runway */
/*========================================*/
/* printf("\thgcg: %g, theta: %g,phi: %g\n",D_cg_above_rwy,Theta*RAD_TO_DEG,Phi*RAD_TO_DEG); */
/* First calculate wheel location w.r.t. cg in body (X-Y-Z) axes... */
sub3( D_gear_v[i], D_cg_rp_body_v, d_wheel_cg_body_v );
/* then converting to local (North-East-Down) axes... */
multtrans3x3by3( T_local_to_body_m, d_wheel_cg_body_v, d_wheel_cg_local_v );
/* Runway axes correction - third element is Altitude, not (-)Z... */
d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* since altitude = -Z */
/* Add wheel offset to cg location in local axes */
add3( d_wheel_cg_local_v, D_cg_rwy_local_v, d_wheel_rwy_local_v );
/* remove Runway axes correction so right hand rule applies */
d_wheel_cg_local_v[2] = -d_wheel_cg_local_v[2]; /* now Z positive down */
/*============================*/
/* Calculate wheel velocities */
/*============================*/
/* contribution due to angular rates */
cross3( Omega_body_v, d_wheel_cg_body_v, temp3a );
/* transform into local axes */
multtrans3x3by3( T_local_to_body_m, temp3a,v_wheel_cg_local_v );
/* plus contribution due to cg velocities */
add3( v_wheel_cg_local_v, V_local_rel_ground_v, v_wheel_local_v );
clear3(f_wheel_local_v);
reaction_normal_force=0;
fgSetBool("/gear/gear[0]/wow", false);
fgSetBool("/gear/gear[1]/wow", false);
fgSetBool("/gear/gear[2]/wow", false);
if( HEIGHT_AGL_WHEEL < 0. )
/*the wheel is underground -- which implies ground contact
so calculate reaction forces */
{
//set the property - weight on wheels
// if (i==0)
// {
// fgSetBool("/gear/gear[0]/wow", true);
// }
// if (i==1)
// {
// fgSetBool("/gear/gear[1]/wow", true);
// }
// if (i==2)
// {
// fgSetBool("/gear/gear[2]/wow", true);
// }
/*===========================================*/
/* Calculate forces & moments for this wheel */
/*===========================================*/
/* Add any anticipation, or frame lead/prediction, here... */
/* no lead used at present */
/* Calculate sideward and forward velocities of the wheel
in the runway plane */
cos_wheel_hdg_angle = cos(caster_angle_rad[i] + Psi);
sin_wheel_hdg_angle = sin(caster_angle_rad[i] + Psi);
v_wheel_forward = v_wheel_local_v[0]*cos_wheel_hdg_angle
+ v_wheel_local_v[1]*sin_wheel_hdg_angle;
v_wheel_sideward = v_wheel_local_v[1]*cos_wheel_hdg_angle
- v_wheel_local_v[0]*sin_wheel_hdg_angle;
/* Calculate normal load force (simple spring constant) */
reaction_normal_force = 0.;
reaction_normal_force = kgear[i]*d_wheel_rwy_local_v[2]
- v_wheel_local_v[2]*cgear[i];
/* printf("\treaction_normal_force: %g\n",reaction_normal_force); */
if (reaction_normal_force > 0.) reaction_normal_force = 0.;
/* to prevent damping component from swamping spring component */
/* Calculate friction coefficients */
if(it_rolls[i])
{
forward_mu = (max_brake_mu[i] - muGear[i])*percent_brake[i] + muGear[i];
abs_v_wheel_sideward = sqrt(v_wheel_sideward*v_wheel_sideward);
sideward_mu = sliding_mu[i];
if (abs_v_wheel_sideward < skid_v)
sideward_mu = (abs_v_wheel_sideward - bkout_v)*beta_mu;
if (abs_v_wheel_sideward < bkout_v) sideward_mu = 0.;
}
else
{
forward_mu=sliding_mu[i];
sideward_mu=sliding_mu[i];
}
/* Calculate foreward and sideward reaction forces */
forward_wheel_force = forward_mu*reaction_normal_force;
sideward_wheel_force = sideward_mu*reaction_normal_force;
if(v_wheel_forward < 0.) forward_wheel_force = -forward_wheel_force;
if(v_wheel_sideward < 0.) sideward_wheel_force = -sideward_wheel_force;
/* printf("\tFfwdgear: %g Fsidegear: %g\n",forward_wheel_force,sideward_wheel_force);
*/
/* Rotate into local (N-E-D) axes */
f_wheel_local_v[0] = forward_wheel_force*cos_wheel_hdg_angle
- sideward_wheel_force*sin_wheel_hdg_angle;
f_wheel_local_v[1] = forward_wheel_force*sin_wheel_hdg_angle
+ sideward_wheel_force*cos_wheel_hdg_angle;
f_wheel_local_v[2] = reaction_normal_force;
/* Convert reaction force from local (N-E-D) axes to body (X-Y-Z) */
mult3x3by3( T_local_to_body_m, f_wheel_local_v, tempF );
/* Calculate moments from force and offsets in body axes */
cross3( d_wheel_cg_body_v, tempF, tempM );
/* Sum forces and moments across all wheels */
if (tempF[0] != 0.0 || tempF[1] != 0.0 || tempF[2] != 0.0) {
fgSetBool("/gear/gear[1]/wow", true);
}
add3( tempF, F_gear_v, F_gear_v );
add3( tempM, M_gear_v, M_gear_v );
}
}
/* printf("\tN: %g,dZrwy: %g dZdotrwy: %g\n",reaction_normal_force,HEIGHT_AGL_WHEEL,v_wheel_cg_local_v[2]); */
/*printf("\tFxgear: %g Fygear: %g, Fzgear: %g\n",F_X_gear,F_Y_gear,F_Z_gear);
printf("\tMgear: %g, Lgear: %g, Ngear: %g\n\n",M_m_gear,M_l_gear,M_n_gear); */
}
}
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;
}
}
}
CL_Vec4<Type> &CL_Vec4<Type>::cross3(const CL_Vec4<Type>& v)
{
*this = cross3(*this, v);
return *this;
}
