double FindRoot(Ffuncclbc *f, double x1, double x2, double prec)
{
double x = -f(x1)*(x2-x1) / (f(x2)-f(x1)) + x1;
if (Abs(f(x))<prec) return x; else
if (Sgn(x)==Sgn(x1)) return FindRoot(f, x, x2, prec); else
return FindRoot(f, x, x1, prec);
return 0;
}
void Vector::Angle(float& u, float& v)
{
Vector n = Unit();
u = asin(n.y);
if(IsZero(n.z)) v = PI/2 * Sgn(n.x);
else if(n.z > 0) v = atan(n.x / n.z);
else if(n.z < 0) v = IsZero(n.x) ? PI : (PI * Sgn(n.x) + atan(n.x / n.z));
}
void BSPVertexContainer<Type, iDimensions>::Initialize(const Vector<Type, iDimensions> &vDirection)
{
bvc_vDirection = vDirection;
// init array of vertices
bvc_aVertices.SetAllocationStep(32);
// find largest axis of direction vector
INDEX iMaxAxis = 0;
Type tMaxAxis = (Type)0;//vDirection(1);
for( INDEX iAxis=1; iAxis<=iDimensions; iAxis++) {
if( Abs(vDirection(iAxis)) > Abs(tMaxAxis) ) {
tMaxAxis = vDirection(iAxis);
iMaxAxis = iAxis;
}
}
/* This assert would seem natural here, but it is not possible because of parallel planes!
// must be greater or equal than minimal max axis of any normalized vector in that space
ASSERT( Abs(tMaxAxis) > (1.0/sqrt(double(iDimensions))-0.01) );
*/
// remember that axis index and sign for sorting
bvc_iMaxAxis = iMaxAxis;
bvc_tMaxAxisSign = Sgn(tMaxAxis);
}
// Calculate business days forward or back depending on a 5-day week.
// Send a negative bdaysaway to calculate days back.
// http://www.smart.net/~mmontes/ushols.html
//==============================================================================================
Date GetDateFromDateBDays(Date startDate, int bdaysaway, UrpCountry country) {
Date fromdate = startDate; // Make working colpy
int dayofweek = DayOfWeek(fromdate);
if (dayofweek == 6 /* Sat */) fromdate+= 1; // Shift to sunday so week calc works
int bdaysleftinweek = 5 - DayOfWeek(fromdate); // Calculate days left in week (backwards)
int weeksinbetween = (bdaysaway - bdaysleftinweek + (bdaysaway > 0? 4 : 0)) / 5; // Substract current week if going forward
bdaysaway = bdaysaway + (weeksinbetween * 2); // 2 days skipped per week
// Holiday general calculator:
// 1 + (Q-1)*7 + (N - DoW(Year,Month,1))%7
// where N is the day of week, Q is what occurence (th) that we want DoW = Day of week for a date
// http://www.tondering.dk/main/index.php/calendar-information
Date edate;
edate = fromdate + bdaysaway;
if (holidays.GetCount() == 0) {
LoadHolidays();
}
// If this is a holiday, skip ahead or back a working day (business day)
if (holidays.At((int)country).Find(edate) != -1) {
return GetDateFromDateBDays(edate, Sgn(bdaysaway), country);
} else {
// If the previous day is sunday and it was a holiday, since we only have official
// non-working holidays in the database, then we calculate the observed holiday
// as being Monday.
// Federal law (5 U.S.C. 6103) establishes the following public holidays for Federal employees. Please note that most Federal employees work on a Monday through Friday schedule. For these employees, when a holiday falls on a nonworkday -- Saturday or Sunday -- the holiday usually is observed on Monday (if the holiday falls on Sunday) or Friday (if the holiday falls on Saturday).
// January 1, 2011 (the legal public holiday for New Year’s Day), falls on a Saturday. For most Federal employees, Friday, December 31, 2010, will be treated as a holiday for pay and leave purposes. (See 5 U.S.C. 6103(b).)
// TODO: Does this work regardless of country?
Date prevfrom_edate = edate - 1;
if (DayOfWeek(prevfrom_edate) == SUNDAY && holidays.At((int)country).Find(prevfrom_edate) != -1) {
return GetDateFromDateBDays(edate, Sgn(bdaysaway), country);
}
// Check if tomorrow is a saturday and a holiday. http://www.opm.gov/Operating_Status_Schedules/fedhol/2011.asp
Date nextafter_edate = edate + 1;
if (DayOfWeek(nextafter_edate) == SATURDAY && holidays.At((int)country).Find(nextafter_edate) != -1) {
return GetDateFromDateBDays(edate, Sgn(bdaysaway), country);
}
}
return edate;
}
int Roots(Ffuncclbc *f, int max_roots, double res, double prec, double x_min, double x_max, double roots[])
{
int fnd = 0; // finded roots
for (double x = x_min; x<=x_max-res; x += res)
{
int sgn1 = Sgn(f(x));
int sgn2 = Sgn(f(x+res));
if (sgn1 == 0) continue; // this root has already been calculated
if (sgn1 != sgn2)
{
if (sgn2 == 0) roots[fnd++] = x+res; else
roots[fnd++] = FindRoot(f, x, x+res, prec);
if (fnd == max_roots) goto end;
}
}
end:
roots[fnd] = 0;
return fnd;
}
/*!
@par Revision history:
- 18.12.2003, c
*/
int32 geme_eig3x3( float64 *out, FMField *mtx )
{
int32 il, dim;
float64 *j, *val;
dim = mtx->nRow;
for (il = 0; il < mtx->nLev; il++) {
j = mtx->val + dim*dim*il;
val = out + dim*il;
switch (dim) {
case 1:
val[0] = j[0];
break;
case 2: {
// See Numerical Recipes.
float64 b, c, q;
b = -j[0] - j[2];
c = j[0] * j[2] - j[1] * j[3];
q = - 0.5 * (b + Sgn(b) * sqrt( b * b - 4.0 * c ));
val[0] = q;
val[1] = c / q;
} break;
case 3: {
// See Numerical Recipes.
float64 a, b, c, q, r, t;
a = -(j[0] + j[4] + j[8]);
b = j[0] * j[4] + j[0] * j[8] + j[4] * j[8]
- j[3] * j[1] - j[6] * j[2] - j[7] * j[5];
c = j[0] * j[5] * j[7] + j[4] * j[6] * j[2] + j[8] * j[1] * j[3]
- j[6] * j[1] * j[5] - j[0] * j[4] * j[8] - j[3] * j[2] * j[7];
q = (a * a - 3.0 * b) / 9.0;
r = (2.0 * a * a * a - 9.0 * a * b + 27.0 * c) / 54.0;
if (((q * q * q) - (r * r)) > MachEps) {
t = acos( r / sqrt( q * q * q ) );
} else {
t = Pi;
}
/* output( "%e %e %e\n", r * r, q * q * q, t ); */
val[0] = -2.0 * sqrt( q ) * cos( (t) / 3.0 ) - a / 3.0;
val[1] = -2.0 * sqrt( q ) * cos( (t+2.0*Pi) / 3.0 ) - a / 3.0;
val[2] = -2.0 * sqrt( q ) * cos( (t-2.0*Pi) / 3.0 ) - a / 3.0;
} break;
default:
errput( ErrHead "ERR_Switch\n" );
}
}
return( RET_OK );
}
inline void renderLine( void *buf,
int byteStride,
int x1, int y1, int x2, int y2,
PIX color )
{
PIX *pix = (PIX*)((char*)buf+byteStride*y1) + x1;
int lg_delta, sh_delta, cycle, lg_step, sh_step;
int alpha, alpha_step, alpha_reset;
int pixStride = byteStride/sizeof(PIX);
AlphaBrush<PIX,THICK> brush( color );
lg_delta = x2 - x1;
sh_delta = y2 - y1;
lg_step = Sgn(lg_delta);
lg_delta = Abs(lg_delta);
sh_step = Sgn(sh_delta);
sh_delta = Abs(sh_delta);
if ( sh_step < 0 ) pixStride = -pixStride;
// in theory should be able to do this with just a single step
// variable - ie: combine cycle and alpha as in wu algorithm
if (sh_delta < lg_delta) {
cycle = lg_delta >> 1;
alpha = ALPHA_MAX >> 1;
alpha_step = -(ALPHA_MAX * sh_delta/(lg_delta+1));
alpha_reset = alpha_step < 0 ? ALPHA_MAX : 0;
int count = lg_step>0 ? x2-x1 : x1-x2;
while ( count-- ) {
brush.ink( pix, pixStride, alpha );
cycle += sh_delta;
alpha += alpha_step;
pix += lg_step;
if (cycle > lg_delta) {
cycle -= lg_delta;
alpha = alpha_reset;
pix += pixStride;
}
}
} else {
int LeviCivitaSymbol_Vargs(int nargs,...){
va_list args;
int* permutation = new int[nargs];
va_start( args, nargs );
for (int i=0;i<nargs;++i) permutation[i] = va_arg(args,int);
va_end( args );
int prod=1;
for (int i = 0;i < nargs-1; ++i)
for (int j = i+1;j < nargs; ++j)
prod * = permutation[j] - permutation[i];
delete[] permutation;
return Sgn(prod);
}
void HermitianSVD
( UpperOrLower uplo, AbstractDistMatrix<F>& A, AbstractDistMatrix<Base<F>>& s,
AbstractDistMatrix<F>& U, AbstractDistMatrix<F>& V )
{
DEBUG_ONLY(CallStackEntry cse("HermitianSVD"))
// Grab an eigenvalue decomposition of A
HermitianEig( uplo, A, s, V );
// Copy V into U (flipping the sign as necessary)
Copy( U, V );
typedef Base<F> Real;
DistMatrix<Real,VR,STAR> sSgn( s );
auto sgnLambda = []( Real sigma ) { return Sgn(sigma,false); };
EntrywiseMap( sSgn, function<Real(Real)>(sgnLambda) );
DiagonalScale( RIGHT, NORMAL, sSgn, U );
// Set the singular values to the absolute value of the eigenvalues
auto absLambda = []( Real sigma ) { return Abs(sigma); };
EntrywiseMap( s, function<Real(Real)>(absLambda) );
// TODO: Descending sort of triplets
}
/**
Convert a vectray of time/points to a piecewise parabolic function.
Time in the first value;
Time values must be monotonically increasing. Results are otherwize undefined.
*/
pararray_vn* vr2pararray(vectray* vr,btreal acceleration)
{
pararray_vn* pavn;
parabolic pa;
int cnt,idx;
btreal t1,t2,t3,x1,x2,x3;
btreal v1,v2,v3,tacc,t1p2,t2p3,tmax;
btreal tf,a,s0,sf;
btreal sv0,svf,sa0,saf,sa0_last,v1_last,saf_last;
btreal s0_prev,tf_prev;
btreal acc,min_acc;
btreal dt,dx,t_last;
vect_n *p0, *pf; //store first and last points of vr
p0 = new_vn(numelements_vr(vr)); // numelements_vr() returns vr->n (columns)
pf = new_vn(numelements_vr(vr));
set_vn(p0, idx_vr(vr, 0));
set_vn(pf, idx_vr(vr, numrows_vr(vr)-1));
//new pararray of size (Viapoints - 1)*2 + 1
pavn = new_pavn(2*numrows_vr(vr) -1 +5, numelements_vr(vr)-1);
tmax = 0;
// For each column of pavn
for (cnt = 0; cnt < pavn->elements; cnt ++) {
acc = fabs(acceleration);
/* First acceleration segment calcs*/
t1 = getval_vn(idx_vr(vr,0),0);
t2 = getval_vn(idx_vr(vr,1),0);
x1 = getval_vn(idx_vr(vr,0),cnt+1);
x2 = getval_vn(idx_vr(vr,1),cnt+1);
dt = t2-t1;
dx = x2-x1;
v1 = 0.0;
if(dt == 0.0)
syslog(LOG_ERR, "vr2pararray(): about to divide by dt = 0.0");
v2 = dx/dt;
min_acc = 8.0 * fabs(dx) / (3.0 * dt*dt);
if(isnan(min_acc))
syslog(LOG_ERR, "vr2pararray(): min_acc is NaN (bad)");
//Make sure initial acceleration is fast enough
if (acc < min_acc) {
//syslog(LOG_ERR,"vr2pararray: Boosting initial acc (%f) to %f", acc, min_acc);
acc = min_acc;
}
tacc = dt - sqrt(dt*dt - 2*fabs(dx)/acc);
saf = x1 + 0.5*acc*tacc*tacc*Sgn(dx); //Final x
v2 = (x2-saf)/(dt-tacc); //final velocity
if(isnan(v2))
syslog(LOG_ERR, "vr2pararray: v2 is NaN (bad)");
sa0 = x1;
tf_prev = s0sfspftf_pb(&pa, 0.0, sa0, saf, v2, tacc); //acc seg starting at time 0.0
add_bseg_pa(pavn->pa[cnt],&pa);
setval_vn(idx_vr(vr,0),cnt+1,x2-dt*v2);
idx = numrows_vr(vr)-1;
/* Last velocity and acceleration segment calcs */
acc = fabs(acceleration);
t1 = getval_vn(idx_vr(vr,idx-1),0);
t2 = getval_vn(idx_vr(vr,idx),0);
x1 = getval_vn(idx_vr(vr,idx-1),cnt+1);
x2 = getval_vn(idx_vr(vr,idx),cnt+1);
dt = t2-t1;
dx = x2-x1;
v1 = dx/dt;
v2 = 0.0;
min_acc = 8.0*fabs(dx)/(3.0*dt*dt);
//Make sure final acceleration is fast enough
if (acc < min_acc) {
acc = min_acc;
//syslog(LOG_ERR,"vr2pararray: Boosting final acc to %f",acc);
}
tacc = dt - sqrt(dt*dt - 2*fabs(dx)/acc);
sa0_last = x2 - 0.5*acc*tacc*tacc*Sgn(dx); //Final x
v1_last = (sa0_last-x1)/(dt-tacc); //final velocity
saf_last = x2;
t_last = tacc;
setval_vn(idx_vr(vr,idx),cnt+1,x1+dt*v1_last);
/*Internal (remaining) segments calcs*/
acc = fabs(acceleration);
for(idx = 1; idx < numrows_vr(vr)-1; idx++) {
/* Extract info */
t1 = getval_vn(idx_vr(vr,idx-1),0);
t2 = getval_vn(idx_vr(vr,idx),0);
t3 = getval_vn(idx_vr(vr,idx+1),0);
x1 = getval_vn(idx_vr(vr,idx-1),cnt+1);
x2 = getval_vn(idx_vr(vr,idx),cnt+1);
x3 = getval_vn(idx_vr(vr,idx+1),cnt+1);
/* Calc some useful values */
//if ((t2-t1) <= 0.0 || (t3-t2) <= 0.0)
//syslog(LOG_ERR,"vr2pararray: Equal time points and unsortet times are not supported.",tacc,idx);
v1 = (x2-x1)/(t2-t1);
v2 = (x3-x2)/(t3-t2);
t1p2 = (t1 + t2)/2;
t2p3 = (t2 + t3)/2;
/* Shrink acceleration if necessary */
tmax = min_bt(2*(t2-t1p2),2*(t2p3-t2));
//vf = v0 + at => t = (vf-v0)/a
tacc = fabs(v2-v1)/acc;
if (tmax < tacc) {
//Need to increase acceleration to make corner
tacc = tmax;
//syslog(LOG_ERR,"vr2pararray: Forcing acc time decrease to %f at point %d.",tacc,idx);
}
/* Calc : tf_prev & saf carry history from prev loops */
sa0 = x2 - v1*(tacc/2); //acc start pos
tf_prev = s0sfspftf_pb(&pa,tf_prev,saf,sa0,v1,t2-tacc/2); //velocity seg
add_bseg_pa(pavn->pa[cnt],&pa);
saf = x2 + v2*(tacc/2); //acc end pos
tf_prev = s0sfspftf_pb(&pa,tf_prev,sa0,saf,v2,t2+tacc/2); //acc seg
add_bseg_pa(pavn->pa[cnt],&pa);
//usleep(1);
}
v2 = 0.0;
tf_prev = s0sfspftf_pb(&pa,tf_prev,saf,sa0_last,v1_last,t3-t_last); //last velocity seg
add_bseg_pa(pavn->pa[cnt],&pa);
tf_prev = s0sfspftf_pb(&pa,tf_prev,sa0_last,saf_last,v2,t3); //acc seg ending at tf
add_bseg_pa(pavn->pa[cnt],&pa);
}
set_vn(idx_vr(vr,0),p0);
set_vn(idx_vr(vr,numrows_vr(vr)-1),pf);
return pavn;
}
//软阈值滤波
void EdgeDetection::PassSoft()
{
//CVideoFrameDoc* pDoc = (CVideoFrameDoc*)GetDocument();
//IplImage *pImage = pDoc->GetImage();
// 图象的高度和宽度
int nWidth = pImage->width;
int nHeight = pImage->height;
int widthStep=pImage->widthStep;
//小波类型及变换层数
Step=2;
m_nSupp=2;
//阈值
double Q=0,t=0;
// 循环变量
int i, j,k,p;
//临时变量
double *pDbTemp;
int W = nWidth >> Step;
int H = nHeight >> Step;
int N = nWidth * nHeight; //图像象素数
// 获取变换的最大层数
int nMaxWLevel = Log2(nWidth);
int nMaxHLevel = Log2(nHeight);
int nMaxLevel;
if (nWidth == 1<<nMaxWLevel && nHeight == 1<<nMaxHLevel)
nMaxLevel = MIN(nMaxWLevel, nMaxHLevel);
// 进行小波变换
int rsl;
rsl = DIBDWTStep(pImage,1,0);
for(i = nHeight / 2;i < nHeight;i ++)
{
// pDbTemp = m_pDbImage + i*sizeImageSave.cx;
for(j = nWidth / 2;j < nWidth;j ++)
Q += fabs(m_pDbImage[i]);
}
Q = Q / (N / 4);
t = 3 * Q;
rsl = DIBDWTStep(pImage,Step - 1,0);
for(k = 0 ;k < Step; k ++)
{
if(H < nHeight)
{
//域值去除高高频和低高频、高低频信息
for(p = 0;p < H; p ++)
{
pDbTemp = m_pDbImage + p*nWidth;
for (i = W; i < W * 2 ; i ++)
{
if(fabs(pDbTemp[i]) < t)
pDbTemp[i] = 0;
else
pDbTemp[i] = Sgn(pDbTemp[i]) * (fabs(pDbTemp[i]) - t);
}
}
for(p = H;p < H * 2; p ++)
{
pDbTemp = m_pDbImage + p*nWidth;
for (i = 0; i < W ; i ++)
{
if(fabs(pDbTemp[i]) < t)
pDbTemp[i] = 0;
else
pDbTemp[i] = Sgn(pDbTemp[i]) * (fabs(pDbTemp[i]) - t);
}
}
for(p = H;p < H * 2; p ++)
{
pDbTemp = m_pDbImage + p*nWidth;
for(i = W; i < W * 2 ; i ++)
{
if(fabs(pDbTemp[i]) < t)
pDbTemp[i] = 0;
else
pDbTemp[i] = Sgn(pDbTemp[i]) * (fabs(pDbTemp[i]) - t);
}
}
H = H * 2;
W = W * 2;
}
}
// 进行小波反变换
for(Step;Step >0;Step --)
{
if (!DWTStep_2D(m_pDbImage, nMaxWLevel-m_nDWTCurDepth, nMaxHLevel-m_nDWTCurDepth,
nMaxWLevel, nMaxHLevel, 1, 1, m_nSupp))
return;
// 是反变换，则当前层数减1
m_nDWTCurDepth --;
}
// 然后，将数据拷贝回原CDib中，并进行相应的数据转换
int lfw = nWidth>>m_nDWTCurDepth, lfh = nHeight>>m_nDWTCurDepth;
for (j=0; j<nHeight; j++)
{
pDbTemp = m_pDbImage + j*nWidth;
//pBits = pDib->GetpBits() + (nHeight-1-j)*sizeImageSave.cx;
for (i=0; i<nWidth; i++)
{
//(pImage->imageData + pImage->widthStep*j)[i]=m_pDbImage[j*nWidth+i];
if (j<lfh && i<lfw)
(pImage->imageData + pImage->widthStep*j)[i] = FloatToChar(m_pDbImage[j*nWidth+i]);
else
(pImage->imageData + pImage->widthStep*j)[i] = FloatToChar(m_pDbImage[j*nWidth+i]) ^ 0x80;
}
}
delete m_pDbImage;
m_pDbImage = NULL;
// 如果小波变换不成功，则直接返回
if (!rsl)
return;
}
/**
see Notebook TH#6 pp146,147
\param seg_acc - Acceleration with which to blend linear segments.
\param q1
\param q2
\param t1
\param t2
\internal chk'd TH 051103 (incomplete)
*/
void CalcSegment(Seg_int *seg,double q1, double q2, double t1, double t2, double t3, double v_prev, double v_next, double seg_acc, int end)
{
double dt,dq,dtn;
double vel,acc1,acc2,dt_vel,dt_acc1,dt_acc2;
double min_acc,use_acc,q_acc1,q_vel,q_acc2;
double max_acc,ac4,sqrtb;
if (t2 <= t1) {
syslog(LOG_ERR, "CalcSeg: Error: Times are out of order!!!! Skipping this segment");
seg->vel = 0.0;
seg->acc1 = 0.0;
seg->acc2 = 0.0;
seg->dt_vel = 0.0;
seg->dt_acc1 = 0.0;
seg->dt_acc2 = 0.0;
return;
}
dt = t2 - t1;
dtn = t3 - t2;
dq = q2 - q1;
#if 0
syslog(LOG_ERR, "CalcSeg: in: q1: %f q2: %f t1: %f t2: %f",q1,q2,t1,t2);
syslog(LOG_ERR, "CalcSeg: in: vprev: %f vnext: %f acc: %f end: %d",v_prev,v_next,seg_acc,end);
#endif
use_acc = fabs(seg_acc); //Force to positive
if (end == 0) {
//Starting segment (Accelerate to the next point)
if(dt > -EPSILON && dt < EPSILON) syslog(LOG_ERR, "btcontrol:CalcSegment:722 dt");
min_acc = 8*fabs(dq)/(3*dt*dt);
if (use_acc < min_acc) { //accelleration must get us to vel_mode at halfway point
use_acc = min_acc;
syslog(LOG_ERR, "CalcSegment: Boosted acceleration to %f to match velocity change",use_acc);
}
if (use_acc != 0.0){
if(use_acc > -EPSILON && use_acc < EPSILON) syslog(LOG_ERR, "btcontrol:CalcSegment:730 use_acc");
dt_acc1 = dt - sqrt(dt*dt - 2*fabs(dq)/use_acc);
}else
dt_acc1 = 0.0;
acc1 = Sgn(dq)*use_acc;
q_acc1 = q1 + 0.5*acc1*dt_acc1*dt_acc1;
//vel = (q2 - q_acc1)/(dt - dt_acc1);
vel = acc1*dt_acc1;
acc2 = Sgn(v_next - vel)*use_acc;
/* Force acceleration high enough to make the corner */
if (fabs(v_next - vel) > dtn*fabs(acc2)/2) {
if(dtn > -EPSILON && dtn < EPSILON) syslog(LOG_ERR, "btcontrol:CalcSegment:741 dtn");
acc2 = (v_next - vel)*2/dtn;
syslog(LOG_ERR, "CalcSegment: Boosted next segment acceleration to %f to fit next side",acc1);
} else if (fabs(v_next - vel) > dt*fabs(acc2)/2) {
if(dt > -EPSILON && dt < EPSILON) syslog(LOG_ERR, "btcontrol:CalcSegment:745 dt");
acc2 = (v_next - vel)*2/dt;
syslog(LOG_ERR, "CalcSegment: Boosted mid segment acceleration to %f to fit this side",acc1);
}
if (acc2 != 0.0) {
if(acc2 > -EPSILON && acc2 < EPSILON) syslog(LOG_ERR, "btcontrol:CalcSegment:750 acc2");
dt_acc2 = (v_next - vel)/acc2;
} else {
dt_acc2 = 0.0;
}
dt_vel = dt - dt_acc1 - dt_acc2/2.0;
if (dt_vel < 0.0) {
dt_vel = 0.0;
syslog(LOG_ERR, "CalcSegment: Init Acc: Not enough acceleration!");
syslog(LOG_ERR, "CalcSegment: Init Acc: dt:%f dt_acc1:%f 0.5*dt_acc2:%f",dt,dt_acc1,dt_acc2/2.0);
}
} else if (end == 1) //Middle segment
{
if(dt > -EPSILON && dt < EPSILON) syslog(LOG_ERR, "btcontrol:CalcSegment:764 dt");
vel = dq/dt;
//acc1 = Sgn(vel - v_prev)*use_acc;
/*
if (fabs(vel - v_prev) > dt*fabs(acc1)/2){
acc1 = (vel - v_prev)*2/dt;
syslog(LOG_ERR, "CalcSegment: Boosted mid segment acceleration to %f to match velocity change",acc1);
}
if (acc1 != 0.0){
dt_acc1 = (vel - v_prev)/acc1;
}
else {
dt_acc1 = 0.0;
}
q_acc1 = q1 + 0.5*acc1*dt_acc1*dt_acc1;
*/
acc1 = seg->acc2;
dt_acc1 = seg->dt_acc2;
acc2 = Sgn(v_next - vel)*use_acc;
/* Force acceleration high enough to make the corner */
if (fabs(v_next - vel) > dtn*fabs(acc2)/2)
{
if(dtn > -EPSILON && dtn < EPSILON) syslog(LOG_ERR, "btcontrol:CalcSegment:787 dtn");
acc2 = (v_next - vel)*2/dtn;
syslog(LOG_ERR, "CalcSegment: Boosted next segment acceleration to %f to fit next side",acc1);
} else if (fabs(v_next - vel) > dt*fabs(acc2)/2)
{
if(dt > -EPSILON && dt < EPSILON) syslog(LOG_ERR, "btcontrol:CalcSegment:792 dt");
acc2 = (v_next - vel)*2/dt;
syslog(LOG_ERR, "CalcSegment: Boosted mid segment acceleration to %f to fit this side",acc1);
}
if (acc2 != 0.0)
{
if(acc2 > -EPSILON && acc2 < EPSILON) syslog(LOG_ERR, "btcontrol:CalcSegment:798 acc2");
dt_acc2 = (v_next - vel)/acc2;
} else
{
dt_acc2 = 0.0;
}
dt_vel = dt - dt_acc1/2 - dt_acc2/2;
if (dt_vel < 0.0)
{
dt_vel = 0.0;
syslog(LOG_ERR, "CalcSegment: Mid segment: Not enough acceleration!");
syslog(LOG_ERR, "CalcSegment: Init Acc: dt:%f 0.5*dt_acc1:%f 0.5*dt_acc2:%f",dt,dt_acc1/2.0,dt_acc2/2.0);
}
}
else if (end == 2) {
//Ending segment (decelerate)
if(dt > -EPSILON && dt < EPSILON) syslog(LOG_ERR, "btcontrol:CalcSegment:816 dt");
min_acc = 8.0*fabs(dq)/(3.0*dt*dt);
if (use_acc < min_acc) { //accelleration must get us to vel_mode at halfway point
use_acc = min_acc;
syslog(LOG_ERR, "CalcSegment: Boosted acc:%f to match end velocity change %f %f",acc1,dq,dt);
//syslog(LOG_ERR, "CalcSegment: %f %f %f %f",q1,q2,t1,t2);
}
if (use_acc != 0.0) {
if(use_acc > -EPSILON && use_acc < EPSILON) syslog(LOG_ERR, "btcontrol:CalcSegment:825 use_acc");
dt_acc2 = dt - sqrt(dt*dt - 2*fabs(dq)/use_acc);
} else {
dt_acc1 = 0.0;
}
acc2 = -1.0*Sgn(dq)*use_acc;
//vel = (dq - 0.5*acc2*dt_acc2*dt_acc2)/(dt - dt_acc2);
vel = -1.0*acc2*dt_acc2;
acc1 = Sgn((vel - v_prev))*use_acc;
if (fabs(vel - v_prev) > dt*fabs(acc1)/2) {
if(dt > -EPSILON && dt < EPSILON) syslog(LOG_ERR, "btcontrol:CalcSegment:835 dt");
acc1 = (vel - v_prev)*2/dt;
syslog(LOG_ERR, "CalcSegment: Boosted mid segment acceleration to %f to match velocity change",acc1);
}
if (acc1 != 0.0) {
if(acc1 > -EPSILON && acc1 < EPSILON) syslog(LOG_ERR, "btcontrol:CalcSegment:840 acc1");
dt_acc1 = (vel - v_prev)/acc1;
} else {
dt_acc1 = 0.0;
}
dt_vel = dt - dt_acc2 - dt_acc1/2;
if (dt_vel < 0.0) {
dt_vel = 0.0;
syslog(LOG_ERR, "CalcSegment: Final Acc: Not enough acceleration!");
syslog(LOG_ERR, "CalcSegment: Final Acc: dt:%f dt_acc1:%f 0.5*dt_acc2:%f",dt,dt_acc1,dt_acc2/2.0);
}
} else {
//Short Segment (accelerate and decellerate with bang bang
if(dt > -EPSILON && dt < EPSILON) syslog(LOG_ERR, "btcontrol:CalcSegment:856 dt");
max_acc = fabs(dq)/(0.25*dt*dt);
if (use_acc > max_acc) {
//accelleration must get us to vel_mode at halfway point
use_acc = max_acc;
syslog(LOG_ERR, "CalcSegment: Boosted acc:%f to match end velocity change %f %f",acc1,dq,dt);
//syslog(LOG_ERR, "CalcSegment: %f %f %f %f",q1,q2,t1,t2);
}
acc2 = -1.0*Sgn(dq)*use_acc;
if (use_acc != 0.0) {
ac4 = acc2*dq*4;
if (acc2*acc2*dt*dt < ac4)
sqrtb = 0.0;
else
sqrtb = sqrt(acc2*acc2*dt*dt - ac4);
if(acc2 > -EPSILON && acc2 < EPSILON) syslog(LOG_ERR, "btcontrol:CalcSegment:872 acc2");
dt_acc1 = acc2*dt/(2.0*acc2)+sqrtb;
} else {
dt_acc1 = 0.0;
}
//vel = (dq - 0.5*acc2*dt_acc2*dt_acc2)/(dt - dt_acc2);
vel = acc2*dt_acc1;
acc1 = -1.0 * acc2;
dt_acc2 = dt_acc1;
dt_vel = dt - dt_acc2 - dt_acc1;
if (dt_vel < 0.0) {
dt_vel = 0.0;
syslog(LOG_ERR, "CalcSegment: Final Acc: Not enough acceleration!");
syslog(LOG_ERR, "CalcSegment: Final Acc: dt:%f dt_acc1:%f 0.5*dt_acc2:%f",dt,dt_acc1,dt_acc2/2.0);
}
}
seg->vel = vel;
seg->acc1 = acc1;
seg->acc2 = acc2;
seg->dt_vel = dt_vel;
seg->dt_acc1 = dt_acc1;
seg->dt_acc2 = dt_acc2;
#if 0
syslog(LOG_ERR, "CalcSeg: Time: dt_acc1: %f dt_vel: %f dt_acc2: %f",dt_acc1,dt_vel,dt_acc2);
syslog(LOG_ERR, "CalcSeg: val: acc1: %f vel: %f acc2: %f",acc1,vel,acc2);
syslog(LOG_ERR, "CalcSeg: ");
#endif
}
void StraightToTargetColumn(int &rnRow, int &rnColumn) {
while (rnColumn != 1)
Print(rnRow, rnColumn += Sgn(1 - rnColumn));
}
void TwoByTwoUpper
( const Real& alpha00,
const Real& alpha01,
const Real& alpha11,
Real& sigmaMax,
Real& sgnMax,
Real& sigmaMin,
Real& sgnMin,
Real& cU,
Real& sU,
Real& cV,
Real& sV )
{
EL_DEBUG_CSE
const Real alpha00Abs = Abs(alpha00);
const Real alpha01Abs = Abs(alpha01);
const Real alpha11Abs = Abs(alpha11);
enum LargestEntry { ALPHA00_LARGEST, ALPHA01_LARGEST, ALPHA11_LARGEST };
LargestEntry largest;
const bool alpha00LargestDiag = ( alpha00Abs >= alpha11Abs );
if( alpha00LargestDiag )
{
largest =
( alpha01Abs > alpha00Abs ? ALPHA01_LARGEST : ALPHA00_LARGEST );
TwoByTwoUpperStandard
( alpha00Abs, alpha01Abs, alpha11Abs,
sigmaMax, sigmaMin, cU, sU, cV, sV );
// sgn(sV) = sgn(phi) = sgn(alpha01/alpha00)
sV = sV*Sgn(alpha01,false)*Sgn(alpha00,false);
cU = cU*Sgn(alpha00*cV+alpha01*sV,false);
sU = sU*Sgn(alpha11,false)*Sgn(sV,false);
}
else
{
largest =
( alpha01Abs > alpha11Abs ? ALPHA01_LARGEST : ALPHA11_LARGEST );
// A decomposition of
//
// | |alpha11|, |alpha01| |
// | 0, |alpha00| |
//
// needs to be transposed and composed with a [0, 1; 1, 0] similarity
// in order to provide a decomposition of
//
// | |alpha00|, |alpha01| |
// | 0, |alpha11| |.
//
// The transpose swaps (cU,sU) with (cV,sV) and the permutation swaps
// cU with sU and cV with sV.
//
TwoByTwoUpperStandard
( alpha11Abs, alpha01Abs, alpha00Abs,
sigmaMax, sigmaMin, sV, cV, sU, cU );
// sgn(cU) = sgn(phi) = sgn(alpha01/alpha11)
cU = cU*Sgn(alpha01,false)*Sgn(alpha11,false);
sV = sV*Sgn(alpha11*sU+alpha01*cU,false);
cV = cV*Sgn(alpha00,false)*Sgn(cU,false);
}
switch( largest )
{
case ALPHA00_LARGEST:
// |alpha00| = sigmaMax cU cV + sigmaMin sU sV
sgnMax = Sgn(cU,false)*Sgn(cV,false)*Sgn(alpha00,false);
break;
case ALPHA01_LARGEST:
// |alpha01| = sigmaMax cU sV - sigmaMin sU cV
sgnMax = Sgn(cU,false)*Sgn(sV,false)*Sgn(alpha01,false);
break;
case ALPHA11_LARGEST:
// |alpha11| = sigmaMax sU sV + sigmaMin cU cV
sgnMax = Sgn(sU,false)*Sgn(sV,false)*Sgn(alpha11,false);
break;
}
// Make use of sgnMin*sgnMax = sgn(det(A))
sgnMin = Sgn(alpha00,false)*Sgn(alpha11,false)*sgnMax;
}
PUBLIC void INTRO_DoGreets(void) {
dword page;
int i;
int loops, f, prevnf;
StarGenFunc sf;
int starsv = 1, gentime;
int CosTb[CTABLEW*2], SinTb[CTABLEW*2];
sint32 angle, va;
int NDots = MAXDOTS;
sint32 destsv = 100;
sint32 destva = 200;
DotPlanes[0] = NEW(MAXDOTS*sizeof(*DotPlanes[0])*4);
if (DotPlanes[0] == NULL)
return;
DotPlanes[1] = DotPlanes[0] + MAXDOTS;
DotPlanes[2] = DotPlanes[1] + MAXDOTS;
DotPlanes[3] = DotPlanes[2] + MAXDOTS;
AddTbl = NEW(sizeof(*AddTbl)*200);
if (AddTbl == NULL) {
DISPOSE(DotPlanes[0]);
return;
}
for (i = 0; i < 200; i++)
AddTbl[i] = 80*i;
Stars = NEW(MAXDOTS*sizeof(*Stars));
if (Stars == NULL) {
DISPOSE(AddTbl);
DISPOSE(DotPlanes[0]);
return;
}
InvTbl = NEW(ZRANGE*sizeof(*InvTbl));
if (InvTbl == NULL) {
DISPOSE(Stars);
DISPOSE(AddTbl);
DISPOSE(DotPlanes[0]);
return;
}
for (i = 0; i < ZRANGE; i++)
InvTbl[i] = (1 << 30)/(ZMIN+i);
FONT_Load(&Font, "fontgrt.fnt");
printf("Font height %d\n", Font.height);
LLS_Init(LLSM_DIRECT, LLSVM_MODEY);
VGA_ZeroPalette();
memset(VGA800x80, 0, 65536);
for (i = 0; i < 16; i++)
VGA_PutColor(i, i*4, i*4, i*4);
for (i = 0; i < 16; i++)
VGA_PutColor(i+16, 16+3*i, i*2, 0);
for (i = 0; i < 16; i++)
VGA_PutColor(i+32, 0, 2*i, 16+i*2);
sf = AddStar5;
gentime = 4*70;
for (i = 0; i < MAXDOTS; i++) {
Stars[i].z = RND_GetNum() % ZRANGE + ZMIN;
sf(Stars + i);
}
prevnf = 1;
Pos = 0;
angle = 0;
va = 0;
page = 0;
loops = 0;
f = 0;
LLK_LastScan = 0;
while (LLK_LastScan != kESC) {
int nf;
if (LLK_Keys[kT]) VGA_SetBorder(63, 0, 0);
if (LLK_Keys[kUARROW])
starsv++;
if (LLK_Keys[kDARROW])
starsv--;
if (LLK_Keys[kLARROW])
va--;
if (LLK_Keys[kRARROW])
va++;
if (LLK_Keys[k1] && SIZEARRAY(StarGen) > 0)
sf = StarGen[0];
if (LLK_Keys[k2] && SIZEARRAY(StarGen) > 1)
sf = StarGen[1];
if (LLK_Keys[k3] && SIZEARRAY(StarGen) > 2)
sf = StarGen[2];
if (LLK_Keys[k4] && SIZEARRAY(StarGen) > 3)
sf = StarGen[3];
if (LLK_Keys[k5] && SIZEARRAY(StarGen) > 4)
sf = StarGen[4];
angle += va;
GenTables(CosTb, SinTb, angle);
page = (page + 80*200);
if (page >= 4*80*200)
page = 0;
VGA_SetPlanes(0x0F);
memset(VGA800x80[0] + page, 0, 80*200);
InitDotPlanes();
{
PStar ps;
ps = Stars;
for (i = 0; i < NDots; i++, ps++) {
int x, y, c;
sint32 z;
z = InvTbl[ps->z-ZMIN];
x = CTABLEW + FPMult(2*ps->x, z);
if (x < 0 || x >= (CTABLEW*2)) {
/*
if (starsv > 0) {
ps->z = ZMAX - RND_GetNum()%1024;
sf(ps);
}
*/
} else {
y = CTABLEW + FPMult(2*ps->y, z);
if (y < 0 || y >= (CTABLEW*2)) {
/*
if (starsv > 0) {
ps->z = ZMAX - RND_GetNum()%1024;
sf(ps);
}
*/
} else {
int rx, ry;
rx = 160 + (CosTb[x] + SinTb[y])/4;
ry = 100 + (CosTb[y] - SinTb[x])/4;
if (rx >= 0 && rx < 320 && ry >= 0 && ry < 200) {
if (ps->z < (ZMIN+ZRANGE/2))
c = 15;
else
c = 15 - (ps->z-ZMIN-ZRANGE/2)*16/(ZRANGE/2);
AddDot(rx, ry, c);
}
}
}
ps->z -= starsv;
if (ps->z < ZMIN) {
ps->z = ZMAX - RND_GetNum()%1024;
sf(ps);
} else if (ps->z > ZMAX) {
ps->z = ZMIN + RND_GetNum()%1024;
sf(ps);
}
}
}
Pos += starsv;
DumpDots(page);
if (f > 1*70) {
Write(page, 0, 0, "Greets from Jare to", 16);
// if (f & 128)
{
Write(page, 0, 60, "All my friends and", 32);
Write(page, 0, 100, "everyone in the scene", 32);
}
}
if (starsv < destsv) starsv++;
else if (starsv > destsv) starsv--;
else {
destsv = RND_GetNum()%200 - 100;
destsv = Sgn(destsv)*200 + destsv;
}
if (va < destva) va++;
else if (va > destva) va--;
else {
destva = RND_GetNum()%200 - 100;
destva = Sgn(destva)*100 + destva;
}
if (gentime-- <= 0) {
gentime = 70 + RND_GetNum()%200;
sf = StarGen[RND_GetNum()%SIZEARRAY(StarGen)];
}
VBL_ChangePage(page);
VGA_SetBorder(0, 0, 0);
nf = VBL_VSync(1);
if (NDots > MAXDOTS/10 && nf > 1 && prevnf > 1)
NDots = NDots - NDots/16;
else if (NDots < MAXDOTS && nf == 1 && prevnf == 1) {
NDots = NDots + NDots/64;
if (NDots > MAXDOTS)
NDots = MAXDOTS;
}
prevnf = nf;
f += nf;
loops++;
}
FONT_End(&Font);
DISPOSE(InvTbl);
DISPOSE(Stars);
DISPOSE(AddTbl);
DISPOSE(DotPlanes[0]);
LLS_Init(LLSM_VIRTUAL, LLSVM_MODE13);
VBL_ChangePage(0);
VGA_ZeroPalette();
VBL_ZeroPalette();
VGA_SetPlanes(0x0F);
memset(VGA800x80[0], 0, 65536);
VBL_VSync(0);
VBL_VSync(2);
}
int GetBezierType ( TPoint2 *pt )
{
double det_012;
double det_123;
double det_013;
double det_023;
int sign_012;
int sign_123;
int sign_013;
int sign_023;
double t;
TPoint2 pt_t;
det_012 = Determinant ( pt [0] , pt [1] , pt [2] );
det_123 = Determinant ( pt [1] , pt [2] , pt [3] );
det_013 = Determinant ( pt [0] , pt [1] , pt [3] );
det_023 = Determinant ( pt [0] , pt [2] , pt [3] );
sign_012 = Sgn ( det_012 );
sign_123 = Sgn ( det_123 );
sign_013 = Sgn ( det_013 );
sign_023 = Sgn ( det_023 );
// First address the cases where 3 or more consecutive control points
// are colinear.
if ( sign_012 == 0 && sign_123 == 0 )
{
if ( sign_013 == 0 )
return kBezTypeStraightLine; // Case E : all 4 points are colinear. Could test for a single point here if necessary
else
return kBezTypeArch; // Points 1 and 2 coincident
}
else if ( sign_012 == 0 )
{
// Case F : first 3 control points are colinear
if ( sign_013 == sign_123 )
return kBezTypeArch;
else
return kBezTypeSingleInflection;
}
else if ( sign_123 == 0 )
{
// Case F : second 3 control points are colinear
if ( sign_023 == sign_012 )
return kBezTypeArch;
else
return kBezTypeSingleInflection;
}
else if ( sign_013 == 0 )
{
// Case G : points 0,1,3 are colinear
short k = PointRelativeToLine ( pt [0] , pt [3] , pt [1] );
if ( k == -1 )
return kBezTypeArch;
else
if ( k == 0 )
return kBezTypeSingleInflection;
else
return CuspValue ( det_012 , det_013 , det_023 );
}
else if ( sign_023 == 0 )
{
// Case G : points 0,2,3 are colinear
short k = PointRelativeToLine ( pt [0] , pt [3] , pt [2] );
if ( k == 1 )
return kBezTypeArch;
else
if ( k == 0 )
return kBezTypeSingleInflection;
else
return CuspValue ( det_012 , det_013 , det_023 );
}
// OK : on to the more interesting stuff. At this point it's known that
// no 3 of the control points are colinear
else if ( sign_012 != sign_123 )
{
// Case A : the control points zig-zag
return kBezTypeSingleInflection;
}
else if ( sign_012 == sign_013 && sign_012 == sign_023 )
{
// Case B : Convex control polygon
return kBezTypeArch;
}
else if ( sign_012 != sign_013 && sign_012 != sign_023 )
{
// Case C : Self-intersecting control polygon
return CuspValue ( det_012 , det_013 , det_023 );
}
else
{
// Case D : Concave control polygon
t = det_013 / ( det_013 - det_023 );
EvaluatePt ( t , pt , &pt_t );
if ( PointRelativeToLine ( pt [0] , pt [3] , pt_t ) == 0 )
{
return CuspValue ( det_012 , det_013 , det_023 );
}
else
return kBezTypeArch;
}
}
