bool OrE::Math::HitTest( Ellipsoid e1, Ellipsoid e2 )
{
// Absolute squared distance vector from e2 to e1
Vec3 vDist = e1.vMiddlePoint - e2.vMiddlePoint;
vDist *= vDist;
// Project ray to both ellipsoid surfaces. All what matters is,
// the the sum of the distances to surface is smaller than the length of
// the distance vector.
// Vec3 vRSum = vDist * ( InvSqrt( vDist.Dot( vDist * e1.vRadiiInvSq ) ) + InvSqrt( vDist.Dot( vDist * e2.vRadiiInvSq ) ) );
// return vRSum.LengthSq() >= vDist.LengthSq();
return InvSqrt( vDist.Dot( e1.vRadiiInvSq ) ) + InvSqrt( vDist.Dot( e2.vRadiiInvSq ) ) >= 1.0f;
}
void GetEstimatedAttitude(imu_t* imu_ptr)
{
uint8_t axis;
int32_t accMag = 0;
float scale, deltaGyroAngle[3];
uint8_t validAcc;
static uint16_t previousT;
uint16_t currentT = micros();
scale = (currentT - previousT) * GYRO_SCALE; // GYRO_SCALE unit: radian/microsecond
previousT = currentT;
// Initialization
for (axis = 0; axis < 3; axis++)
{
deltaGyroAngle[axis] = imu_ptr->gyroADC[axis] * scale; // radian
accLPF32[axis] -= accLPF32[axis]>>ACC_LPF_FACTOR;
accLPF32[axis] += imu_ptr->accADC[axis];
imu_ptr->accSmooth[axis] = accLPF32[axis]>>ACC_LPF_FACTOR;
accMag += (int32_t)imu_ptr->accSmooth[axis]*imu_ptr->accSmooth[axis] ;
}
rotateV(&EstG.V,deltaGyroAngle);
rotateV(&EstM.V,deltaGyroAngle);
accMag = accMag*100/((int32_t)ACC_1G*ACC_1G);
validAcc = 72 < (uint16_t)accMag && (uint16_t)accMag < 133;
// Apply complimentary filter (Gyro drift correction)
// If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
// To do that, we just skip filter, as EstV already rotated by Gyro
for (axis = 0; axis < 3; axis++)
{
if ( validAcc )
EstG.A[axis] = (EstG.A[axis] * GYR_CMPF_FACTOR + imu_ptr->accSmooth[axis]) * INV_GYR_CMPF_FACTOR;
EstG32.A[axis] = EstG.A[axis]; //int32_t cross calculation is a little bit faster than float
EstM.A[axis] = (EstM.A[axis] * GYR_CMPFM_FACTOR + imu_ptr->magADC[axis]) * INV_GYR_CMPFM_FACTOR;
EstM32.A[axis] = EstM.A[axis];
}
// Attitude of the estimated vector
int32_t sqGX_sqGZ = sq(EstG32.V.X) + sq(EstG32.V.Z);
invG = InvSqrt(sqGX_sqGZ + sq(EstG32.V.Y));
att.angle[ROLL] = _atan2(EstG32.V.X , EstG32.V.Z);
att.angle[PITCH] = _atan2(EstG32.V.Y , InvSqrt(sqGX_sqGZ)*sqGX_sqGZ);
att.heading = _atan2(
EstM32.V.Z * EstG32.V.X - EstM32.V.X * EstG32.V.Z,
(EstM.V.Y * sqGX_sqGZ - (EstM32.V.X * EstG32.V.X + EstM32.V.Z * EstG32.V.Z) * EstG.V.Y)*invG );
att.heading /= 10;
}
// pursue behavior
Status PursueBehavior::Execute(void)
{
// get target
const TargetData &targetdata = Database::targetdata.Get(mId);
// get target entity
Entity *targetEntity = Database::entity.Get(targetdata.mTarget);
if (!targetEntity)
return runningTask;
// get pursue behavior template
const PursueBehaviorTemplate &pursue = Database::pursuebehaviortemplate.Get(mId);
// get owner entity
Entity *entity = Database::entity.Get(mId);
// direction to target
Vector2 targetDir(pursue.mOffset.Untransform(TargetDir(pursue.mLeading, entity, targetEntity, targetdata.mOffset)));
// save range
float distSq = targetDir.LengthSq();
// normalize direction
targetDir *= InvSqrt(distSq);
// move towards target
mController->mMove += pursue.mStrength * targetDir;
return runningTask;
}
void PhyCylinder::interact(Graph *pG2)
{
nowPos=pG->nowPos;
nextPos=pG->nextPos;
pos1=pG2->pTransform->pTOmatrix->mMatrixQueue.back();
temp= (nextPos[0]-pos1[12])*(nextPos[0]-pos1[12])+
(nextPos[2]-pos1[14])*(nextPos[2]-pos1[14]);
if(temp>r*r*4 )
return;
ex=nowPos[0]-pos1[12];
ey=nowPos[2]-pos1[14];
templ=ex*ex+ey*ey;
if( temp > templ )
return;
templ=InvSqrt(templ);
temp=ex*templ;
ex=ey*templ;
ey=-temp;
templ=nextPos[0]-nowPos[0];
temp=nextPos[2]-nowPos[2];
templ=ex*templ+ey*temp;
nextPos[0]= nowPos[0]+templ * ex;
nextPos[2]= nowPos[2]+templ * ey;
}
bool OrE::Math::HitDetection( Ellipsoid e1, Ellipsoid e2, Vec3& _vFeedbackLocation )
{
// Absolute squared distance vector from e2 to e1
Vec3 vDist = e1.vMiddlePoint - e2.vMiddlePoint;
Vec3 vDistSq = vDist * vDist;
// Determine the point in the middle between both surfaces.
// Point on surface from e1, which is nearest to the center of e2 is p1 = r1 * -vDist + e1.m.
// Point on surface from e2, which is nearest to the center of e1 is p2 = r2 * vDist + e2.m.
// The point between them is (p1+p2)/2 = ((r2-r1) * vDist + e1.m+e2.m)/2
float r1 = InvSqrt( vDistSq.Dot( e1.vRadiiInvSq ) );
float r2 = InvSqrt( vDistSq.Dot( e2.vRadiiInvSq ) );
_vFeedbackLocation = 0.5f*((r2-r1) * vDist + e1.vMiddlePoint + e2.vMiddlePoint);
// Same as in HitTest
return r1+r2 >= 1.0f;
}
Status EvadeBehavior::Execute(void)
{
// get target
const TargetData &targetdata = Database::targetdata.Get(mId);
// get target entity
Entity *targetEntity = Database::entity.Get(targetdata.mTarget);
if (!targetEntity)
return runningTask;
// get owner entity
Entity *entity = Database::entity.Get(mId);
// get evade behavior template
const EvadeBehaviorTemplate &evade = Database::evadebehaviortemplate.Get(mId);
// target entity transform
const Transform2 &targetTransform = targetEntity->GetTransform();
// evade target's front vector
Vector2 local(targetTransform.Untransform(entity->GetPosition()));
if (local.y > 0)
{
local *= InvSqrt(local.LengthSq());
float dir = local.x > 0 ? 1.0f : -1.0f;
mController->mMove += evade.mStrength * dir * local.y * local.y * local.y * targetTransform.Rotate(Vector2(local.y, -local.x));
}
return runningTask;
}
float4 float4::normalize() const
{
f32 lsqr = LengthSquared();
if(NearZero(lsqr)) { return ZERO; };
f32 recip = InvSqrt(lsqr);
return float4(vec[0]*recip, vec[1]*recip, vec[2]*recip, vec[3]*recip);
};
void angleCalculate()
{
/// Дальше идет суровая математика, в силу которой мы получаем тангаж, крен, а также курс,
/// ... Причем в расчете курса учавствует магнетометр.
// Attitude of the estimated vector
int32_t sqGZ = sq(EstG32.V.Z);
int32_t sqGX = sq(EstG32.V.X);
int32_t sqGY = sq(EstG32.V.Y);
int32_t sqGX_sqGZ = sqGX + sqGZ;
float invmagXZ = InvSqrt(sqGX_sqGZ);
invG = InvSqrt(sqGX_sqGZ + sqGY);
angle[ROLL] = _atan2(EstG32.V.X ,EstG32.V.Z);
angle[PITCH] = - _atan2(EstG32.V.Y , invmagXZ*sqGX_sqGZ);
heading = _atan2(
EstM32.V.Z * EstG32.V.X - EstM32.V.X * EstG32.V.Z,
EstM32.V.Y * invG * sqGX_sqGZ - (EstM32.V.X * EstG32.V.X + EstM32.V.Z * EstG32.V.Z) * invG * EstG32.V.Y );
heading = heading /10;
// vectorMultiply(&EstM32,&EstG32,&headvect);
// heading2=
// axisDekl=_atan2(EstG32.V.X , EstG32.V.Y);
// deklination=_atan(EstG32.V.Z * InvSqrt(sqGX+sqGY));
}
// Return unit length vector in the same direction as given one
CVector3 Normalise( const CVector3& v )
{
TFloat32 lengthSq = v.x*v.x + v.y*v.y + v.z*v.z;
// Ensure vector is not zero length (use BaseMath.h float approx. fn with default epsilon)
if ( gen::IsZero( lengthSq ) )
{
return CVector3(0.0f, 0.0f, 0.0f);
}
else
{
TFloat32 invLength = InvSqrt( lengthSq );
return CVector3(v.x * invLength, v.y * invLength, v.z * invLength);
}
}
// Normalise the quaternion - make it unit length as a 4-vector
void CQuaternion::Normalise()
{
TFloat32 fNormSquared = w*w + x*x + y*y + z*z;
if ( gen::IsZero( fNormSquared ) )
{
w = x = y = z = 0.0f;
}
else
{
TFloat32 fInvLength = InvSqrt( fNormSquared );
w *= fInvLength;
x *= fInvLength;
y *= fInvLength;
z *= fInvLength;
}
}
// Reduce vector to unit length - member function
void CVector3::Normalise()
{
TFloat32 lengthSq = x*x + y*y + z*z;
// Ensure vector is not zero length (use BaseMath.h float approx. fn with default epsilon)
if ( gen::IsZero( lengthSq ) )
{
x = y = z = 0.0f;
}
else
{
TFloat32 invLength = InvSqrt( lengthSq );
x *= invLength;
y *= invLength;
z *= invLength;
}
}
// Return a normalised version of a quaternion (unit length as a 4-vector) - non-member version
CQuaternion Normalise
(
const CQuaternion& quat
)
{
TFloat32 fNormSquared = quat.w*quat.w + quat.x*quat.x + quat.y*quat.y + quat.z*quat.z;
if ( gen::IsZero( fNormSquared ) )
{
return CQuaternion( 0.0f, 0.0f, 0.0f, 0.0f );
}
else
{
TFloat32 fInvLength = InvSqrt( fNormSquared );
return CQuaternion( quat.w*fInvLength, quat.x*fInvLength,
quat.y*fInvLength, quat.z*fInvLength );
}
}
Dvoid Vector4::Normalize()
{
Dfloat lengthsq = x*x + y*y + z*z + w*w;
if ( ::IsZero( lengthsq ) )
{
Zero();
}
else
{
Dfloat factor = InvSqrt( lengthsq );
x *= factor;
y *= factor;
z *= factor;
w *= factor;
}
}
void DMP_Covert_Data(void){
float qtemp[4],norm ; // 四元数
// 注意，这里的计算原来是错误的，但因为 PID参数原因，暂时不改
//DMP_DATA.GYROx 即为直接的角度deg; 而非AD。
DMP_DATA.dmp_gyrox = ((float)DMP_DATA.GYROx)/16.4f; //TOBE FIXED GYROx*M_PI_F/180.0f convert to rad/s
DMP_DATA.dmp_gyroy = ((float)DMP_DATA.GYROy)/16.4f;
DMP_DATA.dmp_gyroz = ((float)DMP_DATA.GYROz)/16.4f;
//acc sensitivity to +/- 4 g
DMP_DATA.dmp_accx = (((float)DMP_DATA.ACCx)/DMP_ACC_SCALE)*ONE_G; //加速度 转成单位： m/S^2
DMP_DATA.dmp_accy = (((float)DMP_DATA.ACCy)/DMP_ACC_SCALE)*ONE_G;
DMP_DATA.dmp_accz = (((float)DMP_DATA.ACCz)/DMP_ACC_SCALE)*ONE_G;
qtemp[0] = (float)DMP_DATA.qw; //提取DMP的四元数
qtemp[1] = (float)DMP_DATA.qx;
qtemp[2] = (float)DMP_DATA.qy;
qtemp[3] = (float)DMP_DATA.qz;
// 四元数归一化
norm = InvSqrt(qtemp[0]*qtemp[0] + qtemp[1]*qtemp[1] + qtemp[2]*qtemp[2] + qtemp[3]*qtemp[3]);
q[0] = qtemp[0] * norm;
q[1] = qtemp[1] * norm;
q[2] = qtemp[2] * norm;
q[3] = qtemp[3] * norm;
DMP_DATA.dmp_roll = (atan2(2.0*(q[0]*q[1] + q[2]*q[3]),
1 - 2.0*(q[1]*q[1] + q[2]*q[2])))* 180/PI;
// we let safe_asin() handle the singularities near 90/-90 in pitch
DMP_DATA.dmp_pitch = asin_s(2.0*(q[0]*q[2] - q[3]*q[1]))* 180/PI;
//注意：此处计算反了，非右手系。
DMP_DATA.dmp_yaw = -atan2(2.0*(q[0]*q[3] + q[1]*q[2]),
1 - 2.0*(q[2]*q[2] + q[3]*q[3]))* 180/PI;
#ifdef YAW_CORRECT
//纠正
DMP_DATA.dmp_yaw=-DMP_DATA.dmp_yaw;
#endif
// gyroxGloble=0.0f*gyrox_val+1.0f*(float)DMP_DATA.GYROx;
// gyrox_val=gyroxGloble;
//
// gyroyGloble=0.0f*gyroy_val+1.0f*(float)DMP_DATA.GYROy;
// gyroy_val=gyroyGloble;
}
void Quaternion::ToAngleAxis(float &angle, Vector3 &axis) const
{
// The quaternion representing the rotation is
// q = cos(A / 2) + sin(A / 2) * (x * i + y * j + z * k)
float norm = Norm();
if (norm > EPSILON)
{
angle = 2.0f * ACos(w);
float invMag = InvSqrt(norm);
axis.x = x * invMag;
axis.y = y * invMag;
axis.z = z * invMag;
}
else
{
// angle is 0 (mod 2 * pi), so use the Z axis so 2D will work
angle = 0.0;
axis.x = 0.0;
axis.y = 0.0;
axis.z = 1.0;
}
}
int force_calc_bon(t_frame pframe, t_files fpkg)
{
int i,j,k,l,N,ci,cj,nqi,id1,id2,id3,id4;
vector *x,*v,dx,dy;
t_vector distx,disty;
double dr,r,r2,r6,r12,ir,ir2,ir6,ir12,E=0.0,rc2;
double ar21,ar22,air21,air22,air11,air12,ang;
double C6ij,C12ij,FORCE,f,df,kc,ddr,fij;
double q2,dfq,fq,fe,phi,cosval,cosv2,dV;
int ist,isl,ds,m;
distx=&dx; disty=&dy;
for(i=0;i<pframe->nr_umols;i++){
for(j=0;j<pframe->nr_mols[i];j++){
// BONDED
for(k=0;k<pframe->mol[pframe->mol_seq[i]]->nr_b;k++){
id1 = j*pframe->mol[pframe->mol_seq[i]]->nr_t + pframe->mol[pframe->mol_seq[i]]->b_seq[2*k ]+pframe->n_start[i];
id2 = j*pframe->mol[pframe->mol_seq[i]]->nr_t + pframe->mol[pframe->mol_seq[i]]->b_seq[2*k+1]+pframe->n_start[i];
r2 = pbc_dx(pframe,id1,id2,dx);
dr = r2*InvSqrt(r2);
// BEGIN HARMONIC
kc = pframe->mol[pframe->mol_seq[i]]->k_bond[2*k];
ddr = dr-pframe->mol[pframe->mol_seq[i]]->k_bond[2*k+1];
df = kc*ddr;
dV = 0.5*df*ddr;
// END HARMONIC
E += dV;
df *= InvSqrt(r2);
for (m=XX; m<=ZZ; m++) {
fij=-df*dx[m];
pframe->f[id1][m]+=fij;
pframe->f[id2][m]-=fij;
}
}
// ANGLES
for(k=0;k<pframe->mol[pframe->mol_seq[i]]->nr_a;k++){
id1 = j*pframe->mol[pframe->mol_seq[i]]->nr_t + pframe->mol[pframe->mol_seq[i]]->a_seq[2*k ]+pframe->n_start[i];
id2 = j*pframe->mol[pframe->mol_seq[i]]->nr_t + pframe->mol[pframe->mol_seq[i]]->a_seq[2*k+1]+pframe->n_start[i];
id3 = j*pframe->mol[pframe->mol_seq[i]]->nr_t + pframe->mol[pframe->mol_seq[i]]->a_seq[2*k+2]+pframe->n_start[i];
ar21 = pbc_dx(pframe,id1,id2,dx);
ar22 = pbc_dx(pframe,id3,id2,dy);
phi = calc_angle(dx,dy,&cosval);
cosv2 = cosval*cosval;
// BEGIN HARMONIC
kc = pframe->mol[pframe->mol_seq[i]]->k_angle[2*k];
ddr = phi-pframe->mol[pframe->mol_seq[i]]->k_angle[2*k+1]*DEG2RAD;
df = kc*ddr;
dV = 0.5*df*ddr;
// END HARMONIC
E += dV;
if(cosv2<1){
double st,sth;
double cik,cii,ckk;
double nrkj2,nrij2;
vector f_i,f_j,f_k;
st = -df*InvSqrt(1.0 - cosv2);
sth = st*cosval;
nrkj2 = iprod(dy,dy);
nrij2 = iprod(dx,dx);
cik = st*InvSqrt(nrkj2*nrij2);
cii = sth/nrij2;
ckk = sth/nrkj2;
for (m=XX; (m<=ZZ); m++) {
f_i[m]=-(cik*dy[m]-cii*dx[m]);
f_k[m]=-(cik*dx[m]-ckk*dy[m]);
f_j[m]=-f_i[m]-f_k[m];
pframe->f[id1][m]+=f_i[m];
pframe->f[id2][m]+=f_j[m];
pframe->f[id3][m]+=f_k[m];
}
}
}
// PROPER DIHEDRALS
E += pdihs(pframe,i,j);
// IMPROPER DIHEDRALS
E += idihs(pframe,i,j);
}
}
pframe->E[2] = E;
pframe->E[0] += E;
return 0;
}
int force_calc_nb(t_frame pframe, t_files fpkg)
{
int i,j,k,N,ci,cj,nqi;
vector *x,*v,distx;
double dr,r,r2,r6,r12,ir,ir2,ir6,ir12,E=0.0,rc2;
double C6ij,C12ij,FORCE,f,df;
double q2,dfq,fq,fe;
int ui,uj,mi,mj,ip,jp;
N = pframe->nr_parts;
pframe->virial = 0.0;
rc2 = pframe->rc2;
nqi = pframe->nr_unique;
for(i=0;i<N;i++){
for(j=i+1;j<N;j++){
mi = pframe->resnr [i];
ui = pframe->restyp[i];
mj = pframe->resnr [j];
uj = pframe->restyp[j];
if( (mi==mj && ui==uj) )
continue;
r2 = pbc_dx(pframe,i,j,distx);
if( r2>rc2 ) //if outside cutoff or same residue
continue;
ir2 = 1.0/r2;
ci = pframe->partyp_key[pframe->partyp[i]];
cj = pframe->partyp_key[pframe->partyp[j]];
if(pframe->C6 [ci*nqi+cj] > 0 && pframe->C12[ci*nqi+cj]>0){
ir6 = ir2*ir2*ir2;
ir12 = ir6*ir6;
C6ij = pframe->C6 [ci*nqi+cj]*ir6;
C12ij= pframe->C12[ci*nqi+cj]*ir12;
E += C12ij-C6ij;
df = (12.0*C12ij - 6.0*C6ij);
f = df*ir2;
for(k=XX;k<=ZZ;k++){
FORCE = f * distx[k] ;
pframe->f[i][k] += FORCE;
pframe->f[j][k] -= FORCE;
}
}
if( pframe->Q [ci*nqi+cj] != 0.0 ){
ir = InvSqrt(r2);
fe = ELUNIT*pframe->Q[ci*nqi+cj]*ir;
E += fe;
f = fe*ir2;
for(k=XX;k<=ZZ;k++){
FORCE = f * distx[k] ;
pframe->f[i][k] += FORCE;
pframe->f[j][k] -= FORCE;
}
}
}
}
if(!isfinite(E))
fatal("NAN ENERGY");
pframe->E[1]=E;
return 0;
}
static boolean RB_IntersectDecalSegment(float x1, float y1, float x2, float y2, line_t *line,
float *x, float *y)
{
float ax, ay;
float bx, by;
float cx, cy;
float dx, dy;
float d, c, s, u;
float newX;
float ab;
ax = x1;
ay = y1;
bx = x2;
by = y2;
cx = line->v1->fx;
cy = line->v1->fy;
dx = line->v2->fx;
dy = line->v2->fy;
if((ax == bx && ay == by) || (cx == dx && cy == dy))
{
// zero length
return false;
}
if((ax == cx && ay == cy) ||
(bx == cx && by == cy) ||
(ax == dx && ay == dy) ||
(bx == dx && by == dy))
{
// shares end point
return false;
}
// translate to origin
bx -= ax; by -= ay;
cx -= ax; cy -= ay;
dx -= ax; dy -= ay;
// normalize
u = bx * bx + by * by;
d = InvSqrt(u);
c = bx * d;
s = by * d;
// rotate points c and d so they're on the positive x axis
newX = cx * c + cy * s;
cy = cy * c - cx * s;
cx = newX;
newX = dx * c + dy * s;
dy = dy * c - dx * s;
dx = newX;
if((cy < 0 && dy < 0) || (cy >= 0 && dy >= 0))
{
// c and d didn't cross
return false;
}
ab = dx + (cx - dx) * dy / (dy - cy);
if(ab < 0 || ab > (u * d))
{
// c and d crosses but outside of points a and b
return false;
}
// lerp
*x = ax + ab * c;
*y = ay + ab * s;
return true;
}
void RB_SpawnWallDecal(mobj_t *mobj)
{
int i;
line_t *line;
rbDecal_t *decal;
rbDecalDef_t *decalDef;
float dx, dy;
float cx, cy;
float fx, fy, fz;
float nx, ny;
float s, c;
float lx1, lx2;
float ly1, ly2;
float d;
float an;
float offs;
float cHeight = 0;
float fHeight = 0;
float size;
if(!rbDecals)
{
return;
}
decalwall = NULL;
decal_x = mobj->x;
decal_y = mobj->y;
decal_z = mobj->z;
if(P_PathTraverse(decal_x, decal_y,
mobj->x + FixedMul(mobj->momx, 10*FRACUNIT),
mobj->y + FixedMul(mobj->momy, 10*FRACUNIT),
PT_ADDLINES, PIT_DecalCheckLine))
{
return;
}
line = decalwall;
if(!line || !(decalDef = RB_GetDecalDef(mobj->type)))
{
return;
}
decal = RB_CreateDecal(decalDef);
decal->x = mobj->x;
decal->y = mobj->y;
decal->z = mobj->z;
decal->type = DCT_WALL;
// look for a sector to stick to
if(line->backsector)
{
decal->stickSector = line->backsector;
if(decal->z > line->backsector->ceilingheight)
{
decal->type = DCT_UPPERWALL;
decal->initialStickZ = line->backsector->ceilingheight;
}
else if(decal->z < line->backsector->floorheight)
{
decal->type = DCT_LOWERWALL;
decal->initialStickZ = line->backsector->floorheight;
}
}
RB_LinkDecal(decal);
dx = line->fdx;
dy = line->fdy;
lx1 = line->v1->fx;
ly1 = line->v1->fy;
lx2 = line->v2->fx;
ly2 = line->v2->fy;
// get line angle (direction)
an = atan2f(dx, dy);
c = cosf(an);
s = sinf(an);
// get line distance
cx = lx1 - FIXED2FLOAT(decal->x);
cy = ly1 - FIXED2FLOAT(decal->y);
d = (dx * cy - dy * cx) * InvSqrt(dx * dx + dy * dy);
// get nudge direction
an -= DEG2RAD(90.0f);
offs = (float)decal->offset / 256.0f;
nx = (d - 0.8f) * sinf(an);
ny = (d - 0.8f) * cosf(an);
/*
2 ----------- 3
| |
| |
| |
| |
| |
1 ----------- 0
*/
decal->numpoints = 4;
size = 16 * decal->scale;
decal->points[0].x = decal->points[3].x = size * s;
decal->points[0].y = decal->points[3].y = size * c;
decal->points[1].x = decal->points[2].x = -size * s;
decal->points[1].y = decal->points[2].y = -size * c;
decal->points[2].z = decal->points[3].z = size;
decal->points[0].z = decal->points[1].z = -size;
fx = FIXED2FLOAT(decal->x);
fy = FIXED2FLOAT(decal->y);
fz = FIXED2FLOAT(decal->z);
decal->points[0].tu = decal->points[3].tu = 0;
decal->points[0].tv = decal->points[1].tv = 0;
decal->points[1].tu = decal->points[2].tu = 1;
decal->points[3].tv = decal->points[2].tv = 1;
if(line->backsector)
{
cHeight = FIXED2FLOAT(line->backsector->ceilingheight);
fHeight = FIXED2FLOAT(line->backsector->floorheight);
}
for(i = 0; i < decal->numpoints; ++i)
{
decal->points[i].x += fx;
decal->points[i].y += fy;
decal->points[i].z += fz;
// nudge decal to be closer to the wall
decal->points[i].x += nx;
decal->points[i].y += ny;
RB_ClampWallDecalToLine(&decal->points[i], line->backsector != NULL,
cHeight, fHeight, fz,
lx1, ly1, lx2, ly2);
// jitter offset a bit
decal->points[i].x -= (nx * offs);
decal->points[i].y -= (ny * offs);
}
RB_RotateDecalTextureCoords(decal);
activedecals++;
}
void Quaternion::Normalize( void )
{
*this *= InvSqrt(NormSqure());
}
void MTX_ToQuaternion(matrix m, float *out)
{
float t, d;
float mx, my, mz;
float m21, m20, m10;
mx = m[ 0];
my = m[ 5];
mz = m[10];
m21 = (m[ 9] - m[ 6]);
m20 = (m[ 8] - m[ 2]);
m10 = (m[ 4] - m[ 1]);
t = 1.0f + mx + my + mz;
if(t > 0)
{
d = 0.5f / (t * InvSqrt(t));
out[0] = m21 * d;
out[1] = m20 * d;
out[2] = m10 * d;
out[3] = 0.25f / d;
}
else if(mx > my && mx > mz)
{
t = 1.0f + mx - my - mz;
d = (t * InvSqrt(t)) * 2;
out[0] = 0.5f / d;
out[1] = m10 / d;
out[2] = m20 / d;
out[3] = m21 / d;
}
else if(my > mz)
{
t = 1.0f + my - mx - mz;
d = (t * InvSqrt(t)) * 2;
out[0] = m10 / d;
out[1] = 0.5f / d;
out[2] = m21 / d;
out[3] = m20 / d;
}
else
{
t = 1.0f + mz - mx - my;
d = (t * InvSqrt(t)) * 2;
out[0] = m20 / d;
out[1] = m21 / d;
out[2] = 0.5f / d;
out[3] = m10 / d;
}
//
// normalize quaternion
// TODO: figure out why InvSqrt produces inaccurate results
// use sqrtf for now
//
d = sqrtf(out[0] * out[0] +
out[1] * out[1] +
out[2] * out[2] +
out[3] * out[3]);
if(d != 0.0f)
{
d = 1.0f / d;
out[0] *= d;
out[1] *= d;
out[2] *= d;
out[3] *= d;
}
}
// player controller ontrol
void PlayerController::Control(float aStep)
{
// get parent entity
Entity *entity = Database::entity.Get(mId);
// get transform
const Transform2 &transform = entity->GetTransform();
// get player controller template
const PlayerControllerTemplate &controllertemplate = Database::playercontrollertemplate.Get(mId);
// TO DO: support multiple players
// TO DO: replace switch statements with behaviors
// set move input
switch (controllertemplate.mMove)
{
case PlayerControllerTemplate::NONE:
mMove.x = mMove.y = 0;
break;
case PlayerControllerTemplate::MOVELOCAL:
mMove.x = input[Input::MOVE_HORIZONTAL];
mMove.y = input[Input::MOVE_VERTICAL];
break;
case PlayerControllerTemplate::MOVEWORLD:
mMove.x = input[Input::MOVE_HORIZONTAL];
mMove.y = input[Input::MOVE_VERTICAL];
mMove = transform.Unrotate(mMove);
break;
case PlayerControllerTemplate::AIMLOCAL:
mMove.x = input[Input::AIM_HORIZONTAL];
mMove.y = input[Input::AIM_VERTICAL];
break;
case PlayerControllerTemplate::AIMWORLD:
mMove.x = input[Input::AIM_HORIZONTAL];
mMove.y = input[Input::AIM_VERTICAL];
mMove = transform.Unrotate(mMove);
break;
case PlayerControllerTemplate::LEFT:
mMove = transform.Unrotate(Vector2(1, 0));
break;
case PlayerControllerTemplate::RIGHT:
mMove = transform.Unrotate(Vector2(-1, 0));
break;
case PlayerControllerTemplate::UP:
mMove = transform.Unrotate(Vector2(0, 1));
break;
case PlayerControllerTemplate::DOWN:
mMove = transform.Unrotate(Vector2(0, -1));
break;
};
// set turn input
switch(controllertemplate.mAim)
{
case PlayerControllerTemplate::NONE:
mAim.x = mAim.y = 0;
mTurn = 0;
break;
case PlayerControllerTemplate::MOVESTEER:
mAim.x = mAim.y = 0;
mTurn = -input[Input::MOVE_HORIZONTAL];
break;
case PlayerControllerTemplate::MOVELOCAL:
mAim.x = input[Input::MOVE_HORIZONTAL];
mAim.y = input[Input::MOVE_VERTICAL];
mTurn = TurnLocal(mAim) / aStep;
break;
case PlayerControllerTemplate::MOVEWORLD:
mAim.x = input[Input::MOVE_HORIZONTAL];
mAim.y = input[Input::MOVE_VERTICAL];
mAim = transform.Unrotate(mAim);
mTurn = TurnLocal(mAim) / aStep;
break;
case PlayerControllerTemplate::MOVECURSOR:
mAim = transform.Untransform(camerapos[1] + Vector2(input[Input::MOVE_HORIZONTAL], input[Input::MOVE_VERTICAL]) * 120 * VIEW_SIZE / 240);
mTurn = TurnLocal(mAim) / aStep;
break;
case PlayerControllerTemplate::AIMSTEER:
mAim.x = mAim.y = 0;
mTurn = -input[Input::AIM_HORIZONTAL];
break;
case PlayerControllerTemplate::AIMLOCAL:
mAim.x = input[Input::AIM_HORIZONTAL];
mAim.y = input[Input::AIM_VERTICAL];
mTurn = TurnLocal(mAim) / aStep;
break;
case PlayerControllerTemplate::AIMWORLD:
mAim.x = input[Input::AIM_HORIZONTAL];
mAim.y = input[Input::AIM_VERTICAL];
mAim = transform.Unrotate(mAim);
mTurn = TurnLocal(mAim) / aStep;
break;
case PlayerControllerTemplate::AIMCURSOR:
mAim = transform.Untransform(camerapos[1] + Vector2(input[Input::AIM_HORIZONTAL], input[Input::AIM_VERTICAL]) * 120 * VIEW_SIZE / 240);
mTurn = TurnLocal(mAim) / aStep;
break;
case PlayerControllerTemplate::LEFT:
mAim = transform.Unrotate(Vector2(1, 0));
mTurn = TurnLocal(mAim) / aStep;
break;
case PlayerControllerTemplate::RIGHT:
mAim = transform.Unrotate(Vector2(-1, 0));
mTurn = TurnLocal(mAim) / aStep;
break;
case PlayerControllerTemplate::UP:
mAim = transform.Unrotate(Vector2(0, 1));
mTurn = TurnLocal(mAim) / aStep;
break;
case PlayerControllerTemplate::DOWN:
mAim = transform.Unrotate(Vector2(0, -1));
mTurn = TurnLocal(mAim) / aStep;
break;
}
// apply scale and add
mMove = mMove * controllertemplate.mScale.p + controllertemplate.mAdd.p;
mTurn = mTurn * controllertemplate.mScale.a + controllertemplate.mAdd.a;
// clamp to limits
float moveSq = mMove.LengthSq();
if (moveSq > 1.0f)
mMove *= InvSqrt(moveSq);
mTurn = Clamp(mTurn, -1.0f, 1.0f);
// set fire input
mFire[0] = input[Input::FIRE_PRIMARY];
mFire[1] = input[Input::FIRE_SECONDARY];
mFire[2] = input[Input::FIRE_CHANNEL3];
mFire[3] = input[Input::FIRE_CHANNEL4];
}
float HGE_InvSqrt(float x) {
return InvSqrt(x);
}
Matrix4 &Matrix4::Orthonormalize()
{
// Algorithm uses Gram-Schmidt orthogonalization. If 'this' matrix is
// M = [m0|m1|m2], then orthonormal output matrix is Q = [q0|q1|q2],
//
// q0 = m0/|m0|
// q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0|
// q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1|
//
// where |V| indicates length of vector V and A*B indicates dot
// product of vectors A and B.
// compute q0
float fInvLength = InvSqrt(
m[0][0] * m[0][0] +
m[1][0] * m[1][0] +
m[2][0] * m[2][0]);
m[0][0] *= fInvLength;
m[1][0] *= fInvLength;
m[2][0] *= fInvLength;
// compute q1
float fDot0 =
m[0][0] * m[0][1] +
m[1][0] * m[1][1] +
m[2][0] * m[2][1];
m[0][1] -= fDot0 * m[0][0];
m[1][1] -= fDot0 * m[1][0];
m[2][1] -= fDot0 * m[2][0];
fInvLength = InvSqrt(
m[0][1] * m[0][1] +
m[1][1] * m[1][1] +
m[2][1] * m[2][1]);
m[0][1] *= fInvLength;
m[1][1] *= fInvLength;
m[2][1] *= fInvLength;
// compute q2
float fDot1 =
m[0][1] * m[0][2] +
m[1][1] * m[1][2] +
m[2][1] * m[2][2];
fDot0 =
m[0][0] * m[0][2] +
m[1][0] * m[1][2] +
m[2][0] * m[2][2];
m[0][2] -= fDot0 * m[0][0] + fDot1 * m[0][1];
m[1][2] -= fDot0 * m[1][0] + fDot1 * m[1][1];
m[2][2] -= fDot0 * m[2][0] + fDot1 * m[2][1];
fInvLength = InvSqrt(
m[0][2] * m[0][2] +
m[1][2] * m[1][2] +
m[2][2] * m[2][2]);
m[0][2] *= fInvLength;
m[1][2] *= fInvLength;
m[2][2] *= fInvLength;
return *this;
}
