// Given the specified position and velocity vectors for a given orbit, retuns the position
// and velocity vectors after a specified time
void PredictPosVelVectors(const VECTOR3 &Pos, const VECTOR3 &Vel, double a, double Mu,
double Time, VECTOR3 &NewPos, VECTOR3 &NewVel, double &NewVelMag)
{
double SqrtMu = sqrt(Mu);
// Variables for computation
double X = (SqrtMu * Time) / a; // Initial guesses for X
double Z = CalcZ(X, a); // and Z
double C, S; // C(Z) and S(Z)
double F, FDot, G, GDot;
// Calculate the X and Z for the specified time of flight
CalcXandZ(X, Z, Pos, Vel, a, Time, SqrtMu);
// Calculate C(Z) and S(Z)
C = CalcC(Z);
S = CalcS(Z);
// Calculate the new position and velocity vectors
F = CalcF(X, C, Mag(Pos));
G = CalcG(Time, X, S, SqrtMu);
NewPos = (Pos * F) + (Vel * G);
FDot = CalcFDot(SqrtMu, Mag(Pos), Mag(NewPos), X, Z, S);
GDot = CalcGDot(X, C, Mag(NewPos));
NewVel = (Pos * FDot) + (Vel * GDot);
NewVelMag = Mag(NewVel);
}
/////////////////////////////////////////////////////////////////////////////
// Smooth path
/////////////////////////////////////////////////////////////////////////////
void K1999::Smooth(int Step)
{
int prev = ((Divs - Step) / Step) * Step;
int prevprev = prev - Step;
int next = Step;
int nextnext = next + Step;
assert(prev >= 0);
//std::cout << Divs << ", " << Step << ", " << prev << ", " << tx.size() << std::endl;
assert(prev < (int)tx.size());
assert(prev < (int)ty.size());
assert(next < (int)tx.size());
assert(next < (int)ty.size());
for (int i = 0; i <= Divs - Step; i += Step)
{
double ri0 = GetRInverse(prevprev, tx[prev], ty[prev], i);
double ri1 = GetRInverse(i, tx[next], ty[next], nextnext);
double lPrev = Mag(tx[i] - tx[prev], ty[i] - ty[prev]);
double lNext = Mag(tx[i] - tx[next], ty[i] - ty[next]);
double TargetRInverse = (lNext * ri0 + lPrev * ri1) / (lNext + lPrev);
double Security = lPrev * lNext / (8 * SecurityR);
AdjustRadius(prev, i, next, TargetRInverse, Security);
prevprev = prev;
prev = i;
next = nextnext;
nextnext = next + Step;
if (nextnext > Divs - Step)
nextnext = 0;
}
}
// Returns true if the IK is complete (either the end effector is at the target or it is not changing from its current position and there is no solution)
bool Skeleton::ProcessIK2D(MatrixBuffer& buffer, XMFLOAT2 rootPos, XMFLOAT2 destPos)
{
// The root position is determined by the pathing algorithm from Project 2 and set here.
// Since the object has already been moved in GraphicsSystem.cpp, the old, new and current root positions are all the same
m_OldJointPositions2D[0] = rootPos;
m_CurrentJointPositions2D[0] = rootPos;
m_TargetJointPositions2D[0] = rootPos;
XMFLOAT2 currentPos;
XMFLOAT2 newPos;
// This loop represents one iteration of Cyclic Coordinate Descent.
// The ProcessIK2D function must be called repeatedly for a solution.
for (int i = (int)m_TargetJointPositions2D.size() - 1; i >= 0; --i)
{
currentPos = CalculateTargetPosition2D();
// If the distance between the current and destination points < epsilon, exit
if (Mag(destPos - currentPos) < m_IKEpsilon)
{
return true;
}
Vector2 Vci = currentPos - m_TargetJointPositions2D[i];;
Vector2 Vdi = destPos - m_TargetJointPositions2D[i];
if (Mag(Vci) <= 0.f || Mag(Vdi) <= 0.f)
{
return false;
}
m_TargetJointRotations2D[i] = AngleInRadians(Vci, Vdi);
// Constrain rotation
if (i == 0) { m_TargetJointRotations2D[i] = PI / 2.f; }
else
{
// Closer to the root => less rotation allowed
float upperBound = PI * (float)i / (float)(m_TargetJointRotations2D.size() - 1);
float lowerBound = -upperBound;
Clamp(m_TargetJointRotations2D[i], lowerBound, upperBound);
}
// Check the new position
newPos = CalculateTargetPosition2D();
}
// If there is little difference between this iteration and the previous one, return
// true to indicate that the IK is complete
if (Mag(newPos - currentPos) < m_IKEpsilon)
{
return true;
}
return false;
}
static int
complex_abs_cmp_internal(Complex * a, Complex * b)
{
double amag = Mag(a),
bmag = Mag(b);
if (amag < bmag)
return -1;
if (amag > bmag)
return 1;
return 0;
}
// Iterative method to calculate new X and Z values for the specified time of flight
static inline void CalcXandZ(double &X, double &Z, const VECTOR3 Pos, const VECTOR3 Vel,
double a, const double Time, const double SqrtMu)
{
const double MAX_ITERS = 10;
double C, S, T, dTdX, DeltaTime, r = Mag(Pos), IterNum = 0;
// These don't change over the iterations
double RVMu = (Pos * Vel) / SqrtMu; // Dot product of position and velocity divided
// by the squareroot of Mu
double OneRA = (1 - (r / a)); // One minus Pos over the semi-major axis
C = CalcC(Z);
S = CalcS(Z);
T = ((RVMu * pow(X, 2) * C) + (OneRA * pow(X, 3) * S) + (r * X)) / SqrtMu;
DeltaTime = Time - T;
// Iterate while the result isn't within tolerances
while (fabs(DeltaTime) > EPSILON && IterNum++ < MAX_ITERS) {
dTdX = ((pow(X, 2) * C) + (RVMu * X * (1 - Z * S)) + (r * (1 - Z * C))) / SqrtMu;
X = X + (DeltaTime / dTdX);
Z = CalcZ(X, a);
C = CalcC(Z);
S = CalcS(Z);
T = ((RVMu * pow(X, 2) * C) + (OneRA * pow(X, 3) * S) + (r * X)) / SqrtMu;
DeltaTime = Time - T;
}
}
Rect::Rect(const vec2& leftBottom, const vec2& rightBottom,
const vec2& leftTop, const vec2& rightTop)
: m_LeftTop(leftTop)
, m_RightTop(rightTop)
, m_LeftBottom(leftBottom)
, m_RightBottom(rightBottom)
, m_Width()
, m_Height()
, m_Diagonal()
{
//[COMMENT] This assert is temporarely disabled because of strange scale issues
/*
//Check if the rect is a rect! (all angles ~90°)
float32 dot1 = Dot(rightTop - leftTop , leftBottom - leftTop);
float32 dot2 = Dot(leftTop - rightTop , rightBottom - rightTop);
float32 dot3 = Dot(rightTop - rightBottom, leftBottom - rightBottom);
float32 tolerance = 0.1f;
ASSERT((abs(dot1) < 0 + tolerance
|| abs(dot2) < 0 + tolerance
|| abs(dot3) < 0 + tolerance)
, _T("The Rect is not a rectangle!!"));*/
m_Width = m_RightTop.x - m_LeftTop.x;
m_Height = m_RightTop.y - m_LeftTop.y;
m_Diagonal = Mag(m_RightBottom - m_LeftTop);
}
template <typename T> ork::TVector2<T> ork::TVector2<T>::Normal() const
{
T fmag = Mag();
fmag = (fmag==T(0)) ? Epsilon() : fmag;
T s = T(1) / fmag;
return TVector2<T>(x * s, y * s);
}
template <typename T> void ork::TVector2<T>::Normalize(void)
{
T distance = T(1) / Mag() ;
x *= distance;
y *= distance;
}
// 下面的函数规范化矢量
CVector3 Normalize(CVector3 vNormal)
{
double Magnitude;
Magnitude = Mag(vNormal); // 获得矢量的长度
vNormal.x /= (float)Magnitude;
vNormal.y /= (float)Magnitude;
vNormal.z /= (float)Magnitude;
return vNormal;
}
// This returns the normal of a vector
void Normalize(float *x,float *y, float *z)
{
double Magnitude; // This holds the magitude
Magnitude = (double)Mag(*x,*y,*z); // Get the magnitude
*x /= (float)Magnitude; // Divide the vector's X by the magnitude
*y /= (float)Magnitude; // Divide the vector's Y by the magnitude
*z /= (float)Magnitude; // Divide the vector's Z by the magnitude
}
void Normalize( CVector3& vNormal)
{
double Magnitude;
Magnitude = Mag(vNormal);
vNormal.x /= (float)Magnitude;
vNormal.y /= (float)Magnitude;
vNormal.z /= (float)Magnitude;
}
AFComplexVector<fftw_complex, double>& AFComplexVector<fftw_complex, double>::ToPolar()
{
assert(!m_bIsPolar);
/*
* It overwrites the input vector U with its own polar
* complex coordinates.
* Here the fftwf_complex type is reinterpretated from Re,Im, to Mag,Phase
*/
fftw_complex cpxTmp;
for(AFSampleCount i = 1; i < (m_smpcLength-1); i++)
{
Mag(cpxTmp) = Magnitude(m_pcpxUf[i]);
Ph(cpxTmp) = -Phase(m_pcpxUf[i]);
Mag(m_pcpxUf[i]) = Mag(cpxTmp);
Ph(m_pcpxUf[i]) = Ph(cpxTmp);
}
m_bIsPolar = true;
return *this;
}
void Rect::SetPoints(const vec2& leftBottom, const vec2& rightBottom,
const vec2& leftTop, const vec2& rightTop)
{
m_LeftBottom = leftBottom;
m_LeftTop = leftTop;
m_RightBottom = rightBottom;
m_RightTop = rightTop;
m_Width = m_RightTop.x - m_LeftTop.x;
m_Height = m_RightTop.y - m_LeftTop.y;
m_Diagonal = Mag(m_RightBottom - m_LeftTop);
}
// This returns the normal of a vector
CVector3 Normalize(CVector3 vNormal)
{
double Magnitude; // This holds the magitude
Magnitude = Mag(vNormal); // Get the magnitude
vNormal.x /= (float)Magnitude; // Divide the vector's X by the magnitude
vNormal.y /= (float)Magnitude; // Divide the vector's Y by the magnitude
vNormal.z /= (float)Magnitude; // Divide the vector's Z by the magnitude
return vNormal; // Return the normal
}
Rect Rect::operator/(float32 constant) const
{
Rect temp;
temp.m_LeftBottom = m_LeftBottom / constant;
temp.m_LeftTop = m_LeftTop / constant;
temp.m_RightBottom = m_RightBottom / constant;
temp.m_RightTop = m_RightTop / constant;
temp.m_Width = temp.m_RightTop.x - temp.m_LeftTop.x;
temp.m_Height = temp.m_RightTop.y - temp.m_LeftTop.y;
temp.m_Diagonal = Mag(temp.m_RightBottom - temp.m_LeftTop);
return temp;
}
Rect& Rect::operator*=(float32 constant)
{
m_LeftBottom *= constant;
m_LeftTop *= constant;
m_RightTop *= constant;
m_RightBottom *= constant;
m_Width = m_RightTop.x - m_LeftTop.x;
m_Height = m_RightTop.y - m_LeftTop.y;
m_Diagonal = Mag(m_RightBottom - m_LeftTop);
return *this;
}
/***************************************************************************\
Scales the point down to a unit vector.
\***************************************************************************/
void inline IMR_3DPoint::Make_Unit(void)
{
FIXEDPNT Length;
// Find length of vector:
Length = Mag();
// Devide everything by the length of the vector:
if (Length)
{
aX = FixedDev(aX, Length);
aY = FixedDev(aY, Length);
aZ = FixedDev(aZ, Length);
}
}
Rect& Rect::operator*=(mat4 matrix)
{
matrix = Transpose(matrix);
vec4 returnVec1 = Mul(vec4(m_LeftBottom.x, m_LeftBottom.y, 0, 0), matrix);
vec4 returnVec2 = Mul(vec4(m_RightBottom.x, m_RightBottom.y, 0, 0), matrix);
vec4 returnVec3 = Mul(vec4(m_LeftTop.x, m_LeftTop.y, 0, 0), matrix);
vec4 returnVec4 = Mul(vec4(m_RightTop.x, m_RightTop.y, 0, 0), matrix);
m_LeftBottom = vec2(returnVec1.x, returnVec1.y);
m_RightBottom = vec2(returnVec2.x , returnVec2.y);
m_LeftTop = vec2(returnVec3.x , returnVec3.y);
m_RightTop = vec2(returnVec4.x , returnVec4.y);
m_Width = m_RightTop.x - m_LeftTop.x;
m_Height = m_RightTop.y - m_LeftTop.y;
m_Diagonal = Mag(m_RightBottom - m_LeftTop);
return *this;
}
/***************************************************************************\
Sets the length of the vector. (More general Make_Unit function).
\***************************************************************************/
void inline IMR_3DPoint::Set_Length(float L)
{
float Length, InvLength = 0;
// Find length of vector:
Length = Mag();
// Avoid a devide-by-zero:
if (Length)
InvLength = (1 / Length) * L;
// Devide everything by the length of the vector and mult by new length:
if (InvLength)
{
aX *= InvLength;
aY *= InvLength;
aZ *= InvLength;
}
}
/***************************************************************************\
Scales the point down to a unit vector.
\***************************************************************************/
void inline IMR_3DPoint::Make_Unit(void)
{
float Length, InvLength = 0;
// Find length of vector:
Length = Mag();
// Avoid a devide-by-zero:
if (Length)
InvLength = 1 / Length;
// Devide everything by the length of the vector:
if (InvLength)
{
aX *= InvLength;
aY *= InvLength;
aZ *= InvLength;
}
}
AFComplexVector<fftwf_complex, float>& AFComplexVector<fftwf_complex, float>::ToRectangular()
{
assert(m_bIsPolar);
/*
* It overwrites the complex 'polar' input vector U with its own rectangular
* complex coordinates.
* Here the fftwf_complex type is reinterpretated from Re,Im, to Mag,Phase
*/
// Devo verificare se la fase della componente DC o Fnyq è pi greca,
// o multiplo intero se la fase non è un multiplo intero di 2pi allora
// la assumo negativa.
//a freq. zero ho solo componente reale
Im(m_pcpxUf[0]) = 0.0;
if( (fmodf(Ph(m_pcpxUf[0]), M_PI) > 1.0) && (fmodf(Ph(m_pcpxUf[0]), M_PI) < 3.0) )
Re(m_pcpxUf[0]) = -Mag(m_pcpxUf[0]); //cioe' se il resto e' non zero,
else
Re(m_pcpxUf[0]) = Mag(m_pcpxUf[0]);
//alla freq. di Nyquist, ho solo valore reale, la Fnyq e' il secondo elemento di h e l'ultimo di hpm
Im(m_pcpxUf[m_smpcLength-1]) = 0.0;
if( (fmodf(Ph(m_pcpxUf[m_smpcLength-1]), M_PI) > 1.0) && (fmodf(Ph(m_pcpxUf[m_smpcLength-1]), M_PI) < 3.0) )
Re(m_pcpxUf[m_smpcLength-1]) = -Mag(m_pcpxUf[m_smpcLength-1]); //cioe' se il resto non e' zero,
else
Re(m_pcpxUf[m_smpcLength-1]) = Mag(m_pcpxUf[m_smpcLength-1]);
fftwf_complex cpxTmp;
for(AFSampleCount i = 1; i < (m_smpcLength-1); i++) // altre frequenze, conto fino ad L2
{
Re(cpxTmp) = Mag(m_pcpxUf[i]) * cosf(Ph(m_pcpxUf[i]));
Im(cpxTmp) = Mag(m_pcpxUf[i]) * sinf(Ph(m_pcpxUf[i]));
Re(m_pcpxUf[i]) = Re(cpxTmp);
Im(m_pcpxUf[i]) = Im(cpxTmp);
}
m_bIsPolar = false;
return *this;
}
double Distance(Vector *a, Vector *b){
Vector temp;
Sub(&temp, a, b);
return Mag(&temp);
}
inline const std::vector<double>& Re() const { return Mag(0); }
inline const double Re(const unsigned int i) const { return Mag(0,i); }
/***************************************************************************\
Returns normalized dot product of points (used as vectors).
\***************************************************************************/
float inline IMR_3DPoint::Dot_Product_Norm(IMR_3DPoint &V)
{
return (aX * V.aX) + (aY * V.aY) + (aZ * V.aZ) / Mag();
}
void CvHOGEvaluator::integralHistogram( const Mat &img, std::vector<Mat> &histogram, Mat &norm, int nbins ) const
{
CV_Assert( img.type() == CV_8U || img.type() == CV_8UC3 );
int x, y, binIdx;
Size gradSize( img.size() );
Size histSize( histogram[0].size() );
Mat grad( gradSize, CV_32F );
Mat qangle( gradSize, CV_8U );
AutoBuffer<int> mapbuf( gradSize.width + gradSize.height + 4 );
int* xmap = (int*) mapbuf + 1;
int* ymap = xmap + gradSize.width + 2;
const int borderType = (int) BORDER_REPLICATE;
for ( x = -1; x < gradSize.width + 1; x++ )
xmap[x] = borderInterpolate( x, gradSize.width, borderType );
for ( y = -1; y < gradSize.height + 1; y++ )
ymap[y] = borderInterpolate( y, gradSize.height, borderType );
int width = gradSize.width;
AutoBuffer<float> _dbuf( width * 4 );
float* dbuf = _dbuf;
Mat Dx( 1, width, CV_32F, dbuf );
Mat Dy( 1, width, CV_32F, dbuf + width );
Mat Mag( 1, width, CV_32F, dbuf + width * 2 );
Mat Angle( 1, width, CV_32F, dbuf + width * 3 );
float angleScale = (float) ( nbins / CV_PI );
for ( y = 0; y < gradSize.height; y++ )
{
const uchar* currPtr = img.data + img.step * ymap[y];
const uchar* prevPtr = img.data + img.step * ymap[y - 1];
const uchar* nextPtr = img.data + img.step * ymap[y + 1];
float* gradPtr = (float*) grad.ptr( y );
uchar* qanglePtr = (uchar*) qangle.ptr( y );
for ( x = 0; x < width; x++ )
{
dbuf[x] = (float) ( currPtr[xmap[x + 1]] - currPtr[xmap[x - 1]] );
dbuf[width + x] = (float) ( nextPtr[xmap[x]] - prevPtr[xmap[x]] );
}
cartToPolar( Dx, Dy, Mag, Angle, false );
for ( x = 0; x < width; x++ )
{
float mag = dbuf[x + width * 2];
float angle = dbuf[x + width * 3];
angle = angle * angleScale - 0.5f;
int bidx = cvFloor( angle );
angle -= bidx;
if( bidx < 0 )
bidx += nbins;
else if( bidx >= nbins )
bidx -= nbins;
qanglePtr[x] = (uchar) bidx;
gradPtr[x] = mag;
}
}
integral( grad, norm, grad.depth() );
float* histBuf;
const float* magBuf;
const uchar* binsBuf;
int binsStep = (int) ( qangle.step / sizeof(uchar) );
int histStep = (int) ( histogram[0].step / sizeof(float) );
int magStep = (int) ( grad.step / sizeof(float) );
for ( binIdx = 0; binIdx < nbins; binIdx++ )
{
histBuf = (float*) histogram[binIdx].data;
magBuf = (const float*) grad.data;
binsBuf = (const uchar*) qangle.data;
memset( histBuf, 0, histSize.width * sizeof ( histBuf[0] ) );
histBuf += histStep + 1;
for ( y = 0; y < qangle.rows; y++ )
{
histBuf[-1] = 0.f;
float strSum = 0.f;
for ( x = 0; x < qangle.cols; x++ )
{
if( binsBuf[x] == binIdx )
strSum += magBuf[x];
histBuf[x] = histBuf[-histStep + x] + strSum;
}
histBuf += histStep;
binsBuf += binsStep;
magBuf += magStep;
}
}
}
/////////////////////////////////////////////////////////////////////////////
// Change lane value to reach a given radius
/////////////////////////////////////////////////////////////////////////////
void K1999::AdjustRadius(int prev, int i, int next, double TargetRInverse, double Security)
{
double OldLane = tLane[i];
double Width = Mag((txLeft[i]-txRight[i]),(tyLeft[i]-tyRight[i]));
//
// Start by aligning points for a reasonable initial lane
//
tLane[i] = (-(ty[next] - ty[prev]) * (txLeft[i] - tx[prev]) +
(tx[next] - tx[prev]) * (tyLeft[i] - ty[prev])) /
( (ty[next] - ty[prev]) * (txRight[i] - txLeft[i]) -
(tx[next] - tx[prev]) * (tyRight[i] - tyLeft[i]));
// the original algorithm allows going outside the track
/*
if (tLane[i] < -0.2)
tLane[i] = -0.2;
else if (tLane[i] > 1.2)
tLane[i] = 1.2;
*/
if (tLane[i] < 0.0)
tLane[i] = 0.0;
else if (tLane[i] > 1.0)
tLane[i] = 1.0;
UpdateTxTy(i);
//
// Newton-like resolution method
//
const double dLane = 0.0001;
double dx = dLane * (txRight[i] - txLeft[i]);
double dy = dLane * (tyRight[i] - tyLeft[i]);
double dRInverse = GetRInverse(prev, tx[i] + dx, ty[i] + dy, next);
if (dRInverse > 0.000000001)
{
tLane[i] += (dLane / dRInverse) * TargetRInverse;
double ExtLane = (SideDistExt + Security) / Width;
double IntLane = (SideDistInt + Security) / Width;
if (ExtLane > 0.5)
ExtLane = 0.5;
if (IntLane > 0.5)
IntLane = 0.5;
if (TargetRInverse >= 0.0)
{
if (tLane[i] < IntLane)
tLane[i] = IntLane;
if (1 - tLane[i] < ExtLane)
{
if (1 - OldLane < ExtLane)
tLane[i] = Min(OldLane, tLane[i]);
else
tLane[i] = 1 - ExtLane;
}
}
else
{
if (tLane[i] < ExtLane)
{
if (OldLane < ExtLane)
tLane[i] = Max(OldLane, tLane[i]);
else
tLane[i] = ExtLane;
}
if (1 - tLane[i] < IntLane)
tLane[i] = 1 - IntLane;
}
}
UpdateTxTy(i);
}
/***************************************************************************\
Returns normalized dot product of points (used as vectors).
\***************************************************************************/
FIXEDPNT inline IMR_3DPoint::Dot_Product_Norm(IMR_3DPoint &V)
{
return (FixedMult(aX, V.aX) + FixedMult(aY, V.aY) + FixedMult(aZ, V.aZ)) / Mag();
}
// mag
float Mag(const Vector4& v)
{
return Mag(v.vec);
}
// normalize
Vector4 Normalize(const Vector4& v)
{
return v/Mag(v);
}
