int GDens1Eval(GDens *ptr, double *x, double *ldens, int flag)
{
int i, r;
double psum, p, xx;
if (flag) /* sample, draw first segment then within the segment */
{
p = GMRFLib_uniform();
r = ptr->n-1;
for(i=0, psum=0.0;i<ptr->n-1;i++)
if ((psum += exp(ptr->elm[i].lp)) > p)
{
r = i; break;
}
p = GMRFLib_uniform();
if (ISZERO(ptr->elm[r].b))
*x = ptr->elm[r].x + p*ptr->elm[r].dx;
else
*x = ptr->elm[r].x + log(1.0-p+p*exp(ptr->elm[r].b*ptr->elm[r].dx))/ptr->elm[r].b;
}
else /* evaluate the density, find the corresponding segment */
{
r = ptr->n-1;
for(i=0;i<ptr->n-1;i++)
if (*x >= ptr->elm[i].x && *x < ptr->elm[i].x+ptr->elm[i].dx)
{
r = i; break;
}
}
if (*x < ptr->xmin || *x > ptr->xmax)
{
fprintf(stdout, "\nGDens1: x=%f is not in [%f,%f], truncate!\n", *x, ptr->xmin, ptr->xmax);
xx = MAX(ptr->xmin, MIN(ptr->xmax, *x));
}
else
xx = *x;
if(ISZERO(ptr->elm[r].b))
*ldens = ptr->elm[r].lp - log(ptr->elm[r].dx);
else
{
double arg = ptr->elm[r].b*exp(ptr->elm[r].b*(xx -ptr->elm[r].x))
/(exp(ptr->elm[r].b*ptr->elm[r].dx)-1.0);
if (ISZERO(arg) || arg <= 0.0)
*ldens = ptr->elm[r].lp - log(ptr->elm[r].dx);
else
*ldens = ptr->elm[r].lp + log(arg);
}
return 0;
}
int GDens1Update(GDens *ptr)
{
/*
update ptr
*/
double lfmax, lpmax, lpsum, psum;
int i, n;
n = ptr->n;
/*
this is just for scaling to prevent overflow/underflow
*/
for(lfmax = ptr->elm[0].lfl, i=0;i<ptr->n;i++)
{
lfmax = MAX(lfmax, ptr->elm[i].lfr);
lfmax = MAX(lfmax, ptr->elm[i].lfl);
}
/*
compute various constants
*/
for(i=0;i<n;i++) ptr->elm[i].b = (ptr->elm[i].lfr-ptr->elm[i].lfl)/ptr->elm[i].dx;
for(i=0;i<n;i++)
{
if (ISZERO(ptr->elm[i].b))
ptr->elm[i].lp = ptr->elm[i].lfl -lfmax + log(ptr->elm[i].dx);
else
{
double arg = (exp(ptr->elm[i].dx*ptr->elm[i].b)-1.0)/ptr->elm[i].b;
if (ISZERO(arg) || arg < 0.)
ptr->elm[i].lp = ptr->elm[i].lfl -lfmax + log(ptr->elm[i].dx);
else
ptr->elm[i].lp = ptr->elm[i].lfl -lfmax + log(arg);
}
}
for(lpmax = ptr->elm[0].lp, i=1;i<n;i++) lpmax = MAX(lpmax, ptr->elm[i].lp);
for(i=0;i<n;i++) ptr->elm[i].lp -= lpmax;
for(i=0, psum=0.0;i<n;i++) psum += exp(ptr->elm[i].lp);
for(lpsum = log(psum), i=0;i<n;i++) ptr->elm[i].lp -= lpsum;
/*
speedup the search for regions and sampling
*/
qsort(ptr->elm, (size_t) ptr->n, sizeof(GDensElm), GDensElmCompare);
return 0;
}
/*
* tan(x) = sin(x) / cos(x)
*/
struct fpn *
fpu_tan(struct fpemu *fe)
{
struct fpn x;
struct fpn s;
struct fpn *r;
if (ISNAN(&fe->fe_f2))
return &fe->fe_f2;
if (ISINF(&fe->fe_f2))
return fpu_newnan(fe);
/* if x is +0/-0, return +0/-0 */
if (ISZERO(&fe->fe_f2))
return &fe->fe_f2;
CPYFPN(&x, &fe->fe_f2);
/* sin(x) */
CPYFPN(&fe->fe_f2, &x);
r = fpu_sin(fe);
CPYFPN(&s, r);
/* cos(x) */
CPYFPN(&fe->fe_f2, &x);
r = fpu_cos(fe);
CPYFPN(&fe->fe_f2, r);
CPYFPN(&fe->fe_f1, &s);
r = fpu_div(fe);
return r;
}
void param_init_number::set_value_inv(const dvector& x, const int& ii)
{
if (ISZERO(scalefactor))
::set_value_inv(*this,(const dvector&)(x),ii);
else
::set_value_inv(*this,(const dvector&)(x),ii,scalefactor);
}
BOOL CNameEvent::Init(LPSTR lpEventName, BOOL bIsCreate)
{
assert(ISNOTNULL(lpEventName));
if (ISNULL(lpEventName))
{
DOLOG("length of Event Name Can't be 0 ");
return FALSE;
}
if (ISZERO(strlen(lpEventName)))
{
DOLOG("length of Event Name Can't be 0 ");
return FALSE;
}
if (ISNOZERO(strcpy_s(m_cbEventName, _countof(m_cbEventName), lpEventName)))
{
DOLOG("copy event name failed !");
return FALSE;
}
if (bIsCreate)
{
return this->CreateEvent();
}
else
{
return this->OpenEvent();
}
}
/*
* arctan(x):
*
* if (x < 0) {
* x = abs(x);
* sign = 1;
* }
* y = arctan(x);
* if (sign) {
* y = -y;
* }
*/
struct fpn *
fpu_atan(struct fpemu *fe)
{
struct fpn a;
struct fpn x;
struct fpn v;
if (ISNAN(&fe->fe_f2))
return &fe->fe_f2;
if (ISZERO(&fe->fe_f2))
return &fe->fe_f2;
CPYFPN(&a, &fe->fe_f2);
if (ISINF(&fe->fe_f2)) {
/* f2 <- pi/2 */
fpu_const(&fe->fe_f2, FPU_CONST_PI);
fe->fe_f2.fp_exp--;
fe->fe_f2.fp_sign = a.fp_sign;
return &fe->fe_f2;
}
fpu_const(&x, FPU_CONST_1);
fpu_const(&fe->fe_f2, FPU_CONST_0);
CPYFPN(&v, &fe->fe_f2);
fpu_cordit1(fe, &x, &a, &fe->fe_f2, &v);
return &fe->fe_f2;
}
void param_init_number::set_value(const dvar_vector& x, const int& ii,
const dvariable& pen)
{
if (ISZERO(scalefactor))
::set_value(*this,x,ii);
else
::set_value(*this,x,ii,scalefactor);
}
void param_init_bounded_matrix::set_value_inv(const dvector& x, const int& ii)
{
if (allocated(*this))
{
if (ISZERO(scalefactor))
::set_value_inv(*this,x,ii,minb,maxb);
else
::set_value_inv(*this,x,ii,minb,maxb,scalefactor);
}
}
void param_init_bounded_matrix::set_value(const dvar_vector& x,
const int& ii, const dvariable& pen)
{
if (allocated(*this))
{
if (ISZERO(scalefactor))
::set_value(*this,x,ii,minb,maxb,pen);
else
::set_value(*this,x,ii,minb,maxb,pen,scalefactor);
}
}
Mobile MoveOnce(Map *map, Mobile mob) {
Collision c;
if (Co_CheckCollision(map, mob, &c)) {
// Move all we can without colliding (a bit less)
if (!ISZERO(c.t0)) {
mob.pos = G_Sum(mob.pos, G_Scale(c.t0 - EPSILON, mob.vel));
}
// Calculate remaining velocity
Vector remaining_vel = G_Scale(1 - (ISZERO(c.t0) ? 0 : c.t0), mob.vel);
// Calculate tangent to the mob circle at the collision point
Vector tangent = G_Perpendicular(G_Sub(c.point, mob.pos));
// Project the remaining velocity over the tangent
mob.vel = G_Project(remaining_vel, tangent);
} else {
mob.pos = G_Sum(mob.pos, mob.vel);
mob.vel = (Vector){0, 0};
}
return mob;
}
/*
NAME {* fdd\_scanallvar *}
SECTION {* fdd *}
SHORT {* Finds one satisfying value of all FDD variables *}
PROTO {* int* fdd_scanallvar(BDD r) *}
DESCR {* Finds one satisfying assignment in {\tt r} of all the defined
FDD variables. Each value is stored in an array which is
returned. The size of this array is exactly the number of
FDD variables defined. It is the user's responsibility to
free this array using {\tt free()}. *}
RETURN {* An array with all satisfying values. If {\tt r} is the trivially
false BDD, then NULL is returned. *}
ALSO {* fdd\_scanvar *}
*/
int* fdd_scanallvar(BDD r)
{
int n;
char *store;
int *res;
BDD p = r;
CHECKa(r,NULL);
if (r == bddfalse)
return NULL;
store = NEW(char,bddvarnum);
for (n=0 ; n<bddvarnum ; n++)
store[n] = 0;
while (!ISCONST(p))
{
if (!ISZERO(LOW(p)))
{
store[bddlevel2var[LEVEL(p)]] = 0;
p = LOW(p);
}
else
{
store[bddlevel2var[LEVEL(p)]] = 1;
p = HIGH(p);
}
}
res = NEW(int, fdvarnum);
for (n=0 ; n<fdvarnum ; n++)
{
int m;
int val=0;
for (m=domain[n].binsize-1 ; m>=0 ; m--)
if ( store[domain[n].ivar[m]] )
val = val*2 + 1;
else
val = val*2;
res[n] = val;
}
free(store);
return res;
}
static int query(int s, double x)
{
register int a, b, c;
for (a = s, b = n - 1; a <= b;) {
c = (a + b) >> 1;
if (ISZERO(pt[c] - x))
return c;
else if (pt[c] < x)
a = c + 1;
else
b = c - 1;
}
return -1;
}
BOOL CDisplayAdapterHelper::InjectSpecificDllAndProcessByNameInner(LPSTR lpAppName, LPSTR lpDllName, BOOL bInjectSafeMode)
{
assert(ISNOTNULL(lpAppName));
if (ISNULL(lpAppName))
{
DOLOG("AppName should not to be null !");
return FALSE;
}
DWORD procid = GetSpecificProcIDByNameEx(lpAppName);
if (ISZERO(procid))
{
DOLOG("GetSpecificProcIDByNameEx Failed");
return FALSE;
}
return InjectSpecificDllAndProcessByIdInner(procid, lpDllName, bInjectSafeMode);
}
Mobile Co_Move(Map *map, Mobile mob) {
Vector orig_vel = mob.vel;
for (int d = 0; d < DEPTH; d++) {
if (ISZERO(G_Length(mob.vel))) return mob;
// Return if the new velocity is against the original velocity.
// This avoids twitching in obtuse angle corners and the like.
// There must be a better solution; calculating the sliding sense as
// well as the direction and having it into acount when projecting the
// remaining velocity.
//
// But this hack is pretty OK.
if (G_Dot(orig_vel, mob.vel) < 0) return mob;
mob = MoveOnce(map, mob);
}
return mob;
}
int GMRFLib_scale_vector(double *x, int n)
{
/*
* x := x/max(x)
*/
int i;
double scale;
if (n <= 0 || !x) {
return GMRFLib_SUCCESS;
}
scale = GMRFLib_max_value(x, n, NULL);
if (!ISZERO(scale)) {
scale = 1.0 / scale;
for (i = 0; i < n; i++) {
x[i] *= scale;
}
}
return GMRFLib_SUCCESS;
}
/*
* x
* arcsin(x) = arctan(---------------)
* sqrt(1 - x^2)
*/
struct fpn *
fpu_asin(struct fpemu *fe)
{
struct fpn x;
struct fpn *r;
if (ISNAN(&fe->fe_f2))
return &fe->fe_f2;
if (ISZERO(&fe->fe_f2))
return &fe->fe_f2;
if (ISINF(&fe->fe_f2))
return fpu_newnan(fe);
CPYFPN(&x, &fe->fe_f2);
/* x^2 */
CPYFPN(&fe->fe_f1, &fe->fe_f2);
r = fpu_mul(fe);
/* 1 - x^2 */
CPYFPN(&fe->fe_f2, r);
fe->fe_f2.fp_sign = 1;
fpu_const(&fe->fe_f1, FPU_CONST_1);
r = fpu_add(fe);
/* sqrt(1-x^2) */
CPYFPN(&fe->fe_f2, r);
r = fpu_sqrt(fe);
/* x/sqrt */
CPYFPN(&fe->fe_f2, r);
CPYFPN(&fe->fe_f1, &x);
r = fpu_div(fe);
/* arctan */
CPYFPN(&fe->fe_f2, r);
return fpu_atan(fe);
}
/** LU decomposition back susbstitution alogrithm for variable object.
\param a A dmatrix containing LU decomposition of input matrix. \f$a\f$.
\param indx Permutation vector from ludcmp.
\param b A dvector containing the RHS, \f$b\f$ of the linear equation
\f$A\cdot X = B\f$, to be solved, and containing on return the solution vector \f$X\f$.
\n\n The implementation of this algorithm was inspired by
"Numerical Recipes in C", 2nd edition,
Press, Teukolsky, Vetterling, Flannery, chapter 2
*/
void lubksb(dvar_matrix a, const ivector& indx,dvar_vector b)
{
int i,ii=0,ip,j,iiflag=0;
dvariable sum;
int lb=a.colmin();
int ub=a.colmax();
for (i=lb;i<=ub;i++)
{
ip=indx(i);
sum=b(ip);
b(ip)=b(i);
if (iiflag)
{
for (j=ii;j<=i-1;j++)
{
sum -= a.elem(i,j)*b.elem(j);
}
}
else if (!ISZERO(value(sum)))
{
ii=i;
iiflag=1;
}
b(i)=sum;
}
for (i=ub;i>=lb;i--)
{
sum=b(i);
for (j=i+1;j<=ub;j++)
{ // !!! remove to show bug
sum -= a.elem(i,j)*b.elem(j);
} // !!! remove to show bug
b.elem(i)=sum/a.elem(i,i);
}
}
/*
* Perform a compare instruction (with or without unordered exception).
* This updates the fcc field in the fsr.
*
* If either operand is NaN, the result is unordered. For cmpe, this
* causes an NV exception. Everything else is ordered:
* |Inf| > |numbers| > |0|.
* We already arranged for fp_class(Inf) > fp_class(numbers) > fp_class(0),
* so we get this directly. Note, however, that two zeros compare equal
* regardless of sign, while everything else depends on sign.
*
* Incidentally, two Infs of the same sign compare equal (per the 80387
* manual---it would be nice if the SPARC documentation were more
* complete).
*/
void
fpu_compare(struct fpemu *fe, int cmpe)
{
register struct fpn *a, *b;
register int cc, r3, r2, r1, r0;
FPU_DECL_CARRY
a = &fe->fe_f1;
b = &fe->fe_f2;
if (ISNAN(a) || ISNAN(b)) {
/*
* In any case, we already got an exception for signalling
* NaNs; here we may replace that one with an identical
* exception, but so what?.
*/
if (cmpe)
fe->fe_cx = FSR_NV;
cc = FSR_CC_UO;
goto done;
}
/*
* Must handle both-zero early to avoid sign goofs. Otherwise,
* at most one is 0, and if the signs differ we are done.
*/
if (ISZERO(a) && ISZERO(b)) {
cc = FSR_CC_EQ;
goto done;
}
if (a->fp_sign) { /* a < 0 (or -0) */
if (!b->fp_sign) { /* b >= 0 (or if a = -0, b > 0) */
cc = FSR_CC_LT;
goto done;
}
} else { /* a > 0 (or +0) */
if (b->fp_sign) { /* b <= -0 (or if a = +0, b < 0) */
cc = FSR_CC_GT;
goto done;
}
}
/*
* Now the signs are the same (but may both be negative). All
* we have left are these cases:
*
* |a| < |b| [classes or values differ]
* |a| > |b| [classes or values differ]
* |a| == |b| [classes and values identical]
*
* We define `diff' here to expand these as:
*
* |a| < |b|, a,b >= 0: a < b => FSR_CC_LT
* |a| < |b|, a,b < 0: a > b => FSR_CC_GT
* |a| > |b|, a,b >= 0: a > b => FSR_CC_GT
* |a| > |b|, a,b < 0: a < b => FSR_CC_LT
*/
#define opposite_cc(cc) ((cc) == FSR_CC_LT ? FSR_CC_GT : FSR_CC_LT)
#define diff(magnitude) (a->fp_sign ? opposite_cc(magnitude) : (magnitude))
if (a->fp_class < b->fp_class) { /* |a| < |b| */
cc = diff(FSR_CC_LT);
goto done;
}
if (a->fp_class > b->fp_class) { /* |a| > |b| */
cc = diff(FSR_CC_GT);
goto done;
}
/* now none can be 0: only Inf and numbers remain */
if (ISINF(a)) { /* |Inf| = |Inf| */
cc = FSR_CC_EQ;
goto done;
}
/*
* Only numbers remain. To compare two numbers in magnitude, we
* simply subtract their mantissas.
*/
FPU_SUBS(r3, a->fp_mant[0], b->fp_mant[0]);
FPU_SUBCS(r2, a->fp_mant[1], b->fp_mant[1]);
FPU_SUBCS(r1, a->fp_mant[2], b->fp_mant[2]);
FPU_SUBC(r0, a->fp_mant[3], b->fp_mant[3]);
if (r0 < 0) /* underflow: |a| < |b| */
cc = diff(FSR_CC_LT);
else if ((r0 | r1 | r2 | r3) != 0) /* |a| > |b| */
cc = diff(FSR_CC_GT);
else
cc = FSR_CC_EQ; /* |a| == |b| */
done:
fe->fe_fsr = (fe->fe_fsr & ~FSR_FCC) | (cc << FSR_FCC_SHIFT);
}
/*
* Perform a compare instruction (with or without unordered exception).
* This updates the fcc field in the fsr.
*
* If either operand is NaN, the result is unordered. For cmpe, this
* causes an NV exception. Everything else is ordered:
* |Inf| > |numbers| > |0|.
* We already arranged for fp_class(Inf) > fp_class(numbers) > fp_class(0),
* so we get this directly. Note, however, that two zeros compare equal
* regardless of sign, while everything else depends on sign.
*
* Incidentally, two Infs of the same sign compare equal (per the 80387
* manual---it would be nice if the SPARC documentation were more
* complete).
*/
void
__fpu_compare(struct fpemu *fe, int cmpe, int fcc)
{
struct fpn *a, *b;
int cc;
FPU_DECL_CARRY
a = &fe->fe_f1;
b = &fe->fe_f2;
if (ISNAN(a) || ISNAN(b)) {
/*
* In any case, we already got an exception for signalling
* NaNs; here we may replace that one with an identical
* exception, but so what?.
*/
if (cmpe)
fe->fe_cx = FSR_NV;
cc = FSR_CC_UO;
goto done;
}
/*
* Must handle both-zero early to avoid sign goofs. Otherwise,
* at most one is 0, and if the signs differ we are done.
*/
if (ISZERO(a) && ISZERO(b)) {
cc = FSR_CC_EQ;
goto done;
}
if (a->fp_sign) { /* a < 0 (or -0) */
if (!b->fp_sign) { /* b >= 0 (or if a = -0, b > 0) */
cc = FSR_CC_LT;
goto done;
}
} else { /* a > 0 (or +0) */
if (b->fp_sign) { /* b <= -0 (or if a = +0, b < 0) */
cc = FSR_CC_GT;
goto done;
}
}
/*
* Now the signs are the same (but may both be negative). All
* we have left are these cases:
*
* |a| < |b| [classes or values differ]
* |a| > |b| [classes or values differ]
* |a| == |b| [classes and values identical]
*
* We define `diff' here to expand these as:
*
* |a| < |b|, a,b >= 0: a < b => FSR_CC_LT
* |a| < |b|, a,b < 0: a > b => FSR_CC_GT
* |a| > |b|, a,b >= 0: a > b => FSR_CC_GT
* |a| > |b|, a,b < 0: a < b => FSR_CC_LT
*/
#define opposite_cc(cc) ((cc) == FSR_CC_LT ? FSR_CC_GT : FSR_CC_LT)
#define diff(magnitude) (a->fp_sign ? opposite_cc(magnitude) : (magnitude))
if (a->fp_class < b->fp_class) { /* |a| < |b| */
cc = diff(FSR_CC_LT);
goto done;
}
if (a->fp_class > b->fp_class) { /* |a| > |b| */
cc = diff(FSR_CC_GT);
goto done;
}
/* now none can be 0: only Inf and numbers remain */
if (ISINF(a)) { /* |Inf| = |Inf| */
cc = FSR_CC_EQ;
goto done;
}
/*
* Only numbers remain. To compare two numbers in magnitude, we
* simply subtract them.
*/
a = __fpu_sub(fe);
if (a->fp_class == FPC_ZERO)
cc = FSR_CC_EQ;
else
cc = diff(FSR_CC_GT);
done:
fe->fe_fsr = (fe->fe_fsr & fcc_nmask[fcc]) |
((u_long)cc << fcc_shift[fcc]);
}
void MeshSkin::computeBounds()
{
// Find the offset of the blend indices and blend weights within the mesh vertices
int blendIndexOffset = -1;
int blendWeightOffset = -1;
for (unsigned int i = 0, count = _mesh->getVertexElementCount(); i < count; ++i)
{
const VertexElement& e = _mesh->getVertexElement(i);
switch (e.usage)
{
case BLENDINDICES:
blendIndexOffset = i;
break;
case BLENDWEIGHTS:
blendWeightOffset = i;
break;
}
}
if (blendIndexOffset == -1 || blendWeightOffset == -1)
{
// Need blend indices and blend weights to calculate skinned bounding volume
return;
}
LOG(2, "Computing bounds for skin of mesh: %s\n", _mesh->getId().c_str());
// Get the root joint
if (_joints.size() == 0)
return;
Node* rootJoint = _joints[0];
Node* parent = rootJoint->getParent();
while (parent)
{
// Is this parent in the list of joints that form the skeleton?
// If not, then it's simply a parent node to the root joint
if (find(_joints.begin(), _joints.end(), parent) != _joints.end())
{
rootJoint = parent;
}
parent = parent->getParent();
}
// If the root joint has a parent node, temporarily detach it so that its transform is
// not included in the bounding volume calculation below
Node* rootJointParent = rootJoint->getParent();
if (rootJointParent)
{
rootJointParent->removeChild(rootJoint);
}
unsigned int jointCount = _joints.size();
unsigned int vertexCount = _mesh->getVertexCount();
LOG(3, " %u joints found.\n", jointCount);
std::vector<AnimationChannel*> channels;
std::vector<Node*> channelTargets;
std::vector<Curve*> curves;
std::vector<Vector3> vertices;
_jointBounds.resize(jointCount);
// Construct a list of all animation channels that target the joints affecting this mesh skin
LOG(3, " Collecting animations...\n");
LOG(3, " 0%%\r");
for (unsigned int i = 0; i < jointCount; ++i)
{
Node* joint = _joints[i];
// Find all animations that target this joint
Animations* animations = GPBFile::getInstance()->getAnimations();
for (unsigned int j = 0, animationCount = animations->getAnimationCount(); j < animationCount; ++j)
{
Animation* animation = animations->getAnimation(j);
for (unsigned int k = 0, channelCount = animation->getAnimationChannelCount(); k < channelCount; ++k)
{
AnimationChannel* channel = animation->getAnimationChannel(k);
if (channel->getTargetId() == joint->getId())
{
if (find(channels.begin(), channels.end(), channel) == channels.end())
{
channels.push_back(channel);
channelTargets.push_back(joint);
}
}
}
}
// Calculate the local bounding volume for this joint
vertices.clear();
BoundingVolume jointBounds;
jointBounds.min.set(FLT_MAX, FLT_MAX, FLT_MAX);
jointBounds.max.set(-FLT_MAX, -FLT_MAX, -FLT_MAX);
for (unsigned int j = 0; j < vertexCount; ++j)
{
const Vertex& v = _mesh->getVertex(j);
if ((v.blendIndices.x == i && !ISZERO(v.blendWeights.x)) ||
(v.blendIndices.y == i && !ISZERO(v.blendWeights.y)) ||
(v.blendIndices.z == i && !ISZERO(v.blendWeights.z)) ||
(v.blendIndices.w == i && !ISZERO(v.blendWeights.w)))
{
vertices.push_back(v.position);
// Update box min/max
if (v.position.x < jointBounds.min.x)
jointBounds.min.x = v.position.x;
if (v.position.y < jointBounds.min.y)
jointBounds.min.y = v.position.y;
if (v.position.z < jointBounds.min.z)
jointBounds.min.z = v.position.z;
if (v.position.x > jointBounds.max.x)
jointBounds.max.x = v.position.x;
if (v.position.y > jointBounds.max.y)
jointBounds.max.y = v.position.y;
if (v.position.z > jointBounds.max.z)
jointBounds.max.z = v.position.z;
}
}
if (vertices.size() > 0)
{
// Compute center point
Vector3::add(jointBounds.min, jointBounds.max, &jointBounds.center);
jointBounds.center.scale(0.5f);
// Compute radius
for (unsigned int j = 0, jointVertexCount = vertices.size(); j < jointVertexCount; ++j)
{
float d = jointBounds.center.distanceSquared(vertices[j]);
if (d > jointBounds.radius)
jointBounds.radius = d;
}
jointBounds.radius = sqrt(jointBounds.radius);
}
_jointBounds[i] = jointBounds;
LOG(3, " %d%%\r", (int)((float)(i+1) / (float)jointCount * 100.0f));
}
LOG(3, "\n");
unsigned int channelCount = channels.size();
// Create a Curve for each animation channel
float maxDuration = 0.0f;
LOG(3, " Building animation curves...\n");
LOG(3, " 0%%\r");
for (unsigned int i = 0; i < channelCount; ++i)
{
AnimationChannel* channel = channels[i];
const std::vector<float>& keyTimes = channel->getKeyTimes();
unsigned int keyCount = keyTimes.size();
if (keyCount == 0)
continue;
// Create a curve for this animation channel
Curve* curve = NULL;
switch (channel->getTargetAttribute())
{
case Transform::ANIMATE_SCALE_ROTATE_TRANSLATE:
curve = new Curve(keyCount, 10);
curve->setQuaternionOffset(3);
break;
}
if (curve == NULL)
{
// Unsupported/not implemented curve type
continue;
}
// Copy key values into a temporary array
unsigned int keyValuesCount = channel->getKeyValues().size();
float* keyValues = new float[keyValuesCount];
for (unsigned int j = 0; j < keyValuesCount; ++j)
keyValues[j] = channel->getKeyValues()[j];
// Determine animation duration
float startTime = keyTimes[0];
float duration = keyTimes[keyCount-1] - startTime;
if (duration > maxDuration)
maxDuration = duration;
if (duration > 0.0f)
{
// Set curve points
float* keyValuesPtr = keyValues;
for (unsigned int j = 0; j < keyCount; ++j)
{
// Store time normalized, between 0-1
float t = (keyTimes[j] - startTime) / duration;
// Set the curve point
// TODO: Handle other interpolation types
curve->setPoint(j, t, keyValuesPtr, gameplay::Curve::LINEAR);
// Move to the next point on the curve
keyValuesPtr += curve->getComponentCount();
}
curves.push_back(curve);
}
delete[] keyValues;
keyValues = NULL;
LOG(3, " %d%%\r", (int)((float)(i+1) / (float)channelCount * 100.0f));
}
LOG(3, "\n");
// Compute a total combined bounding volume for the MeshSkin that contains all possible
// vertex positions for all animations targeting the skin. This rough approximation allows
// us to store a volume that can be used for rough intersection tests (such as for visibility
// determination) efficiently at runtime.
// Backup existing node transforms so we can restore them when we are finished
Matrix* oldTransforms = new Matrix[jointCount];
for (unsigned int i = 0; i < jointCount; ++i)
{
memcpy(oldTransforms[i].m, _joints[i]->getTransformMatrix().m, 16 * sizeof(float));
}
float time = 0.0f;
float srt[10];
Matrix temp;
Matrix* jointTransforms = new Matrix[jointCount];
_mesh->bounds.min.set(FLT_MAX, FLT_MAX, FLT_MAX);
_mesh->bounds.max.set(-FLT_MAX, -FLT_MAX, -FLT_MAX);
_mesh->bounds.center.set(0, 0, 0);
_mesh->bounds.radius = 0;
Vector3 skinnedPos;
Vector3 tempPos;
LOG(3, " Evaluating joints...\n");
LOG(3, " 0%%\r");
BoundingVolume finalBounds;
while (time <= maxDuration)
{
// Evaluate joint transforms at this time interval
for (unsigned int i = 0, curveCount = curves.size(); i < curveCount; ++i)
{
Node* joint = channelTargets[i];
Curve* curve = curves[i];
// Evalulate the curve at this time to get the new value
float tn = time / maxDuration;
if (tn > 1.0f)
tn = 1.0f;
curve->evaluate(tn, srt);
// Update the joint's local transform
Matrix::createTranslation(srt[7], srt[8], srt[9], temp.m);
temp.rotate(*((Quaternion*)&srt[3]));
temp.scale(srt[0], srt[1], srt[2]);
joint->setTransformMatrix(temp.m);
}
// Store the final matrix pallette of resovled world space joint matrices
std::vector<Matrix>::const_iterator bindPoseItr = _bindPoses.begin();
for (unsigned int i = 0; i < jointCount; ++i, bindPoseItr++)
{
BoundingVolume bounds = _jointBounds[i];
if (ISZERO(bounds.radius))
continue;
Matrix& m = jointTransforms[i];
Matrix::multiply(_joints[i]->getWorldMatrix().m, bindPoseItr->m, m.m);
Matrix::multiply(m.m, _bindShape, m.m);
// Get a world-space bounding volume for this joint
bounds.transform(m);
if (ISZERO(finalBounds.radius))
finalBounds = bounds;
else
finalBounds.merge(bounds);
}
// Increment time by 1/30th of second (~ 33 ms)
if (time < maxDuration && (time + 33.0f) > maxDuration)
time = maxDuration;
else
time += 33.0f;
LOG(3, " %d%%\r", (int)(time / maxDuration * 100.0f));
}
LOG(3, "\n");
// Update the bounding sphere for the mesh
_mesh->bounds = finalBounds;
// Restore original joint transforms
for (unsigned int i = 0; i < jointCount; ++i)
{
_joints[i]->setTransformMatrix(oldTransforms[i].m);
}
// Cleanup
for (unsigned int i = 0, curveCount = curves.size(); i < curveCount; ++i)
{
delete curves[i];
}
delete[] oldTransforms;
delete[] jointTransforms;
// Restore removed joints
if (rootJointParent)
{
rootJointParent->addChild(rootJoint);
}
}
/*
* Our task is to calculate the square root of a floating point number x0.
* This number x normally has the form:
*
* exp
* x = mant * 2 (where 1 <= mant < 2 and exp is an integer)
*
* This can be left as it stands, or the mantissa can be doubled and the
* exponent decremented:
*
* exp-1
* x = (2 * mant) * 2 (where 2 <= 2 * mant < 4)
*
* If the exponent `exp' is even, the square root of the number is best
* handled using the first form, and is by definition equal to:
*
* exp/2
* sqrt(x) = sqrt(mant) * 2
*
* If exp is odd, on the other hand, it is convenient to use the second
* form, giving:
*
* (exp-1)/2
* sqrt(x) = sqrt(2 * mant) * 2
*
* In the first case, we have
*
* 1 <= mant < 2
*
* and therefore
*
* sqrt(1) <= sqrt(mant) < sqrt(2)
*
* while in the second case we have
*
* 2 <= 2*mant < 4
*
* and therefore
*
* sqrt(2) <= sqrt(2*mant) < sqrt(4)
*
* so that in any case, we are sure that
*
* sqrt(1) <= sqrt(n * mant) < sqrt(4), n = 1 or 2
*
* or
*
* 1 <= sqrt(n * mant) < 2, n = 1 or 2.
*
* This root is therefore a properly formed mantissa for a floating
* point number. The exponent of sqrt(x) is either exp/2 or (exp-1)/2
* as above. This leaves us with the problem of finding the square root
* of a fixed-point number in the range [1..4).
*
* Though it may not be instantly obvious, the following square root
* algorithm works for any integer x of an even number of bits, provided
* that no overflows occur:
*
* let q = 0
* for k = NBITS-1 to 0 step -1 do -- for each digit in the answer...
* x *= 2 -- multiply by radix, for next digit
* if x >= 2q + 2^k then -- if adding 2^k does not
* x -= 2q + 2^k -- exceed the correct root,
* q += 2^k -- add 2^k and adjust x
* fi
* done
* sqrt = q / 2^(NBITS/2) -- (and any remainder is in x)
*
* If NBITS is odd (so that k is initially even), we can just add another
* zero bit at the top of x. Doing so means that q is not going to acquire
* a 1 bit in the first trip around the loop (since x0 < 2^NBITS). If the
* final value in x is not needed, or can be off by a factor of 2, this is
* equivalant to moving the `x *= 2' step to the bottom of the loop:
*
* for k = NBITS-1 to 0 step -1 do if ... fi; x *= 2; done
*
* and the result q will then be sqrt(x0) * 2^floor(NBITS / 2).
* (Since the algorithm is destructive on x, we will call x's initial
* value, for which q is some power of two times its square root, x0.)
*
* If we insert a loop invariant y = 2q, we can then rewrite this using
* C notation as:
*
* q = y = 0; x = x0;
* for (k = NBITS; --k >= 0;) {
* #if (NBITS is even)
* x *= 2;
* #endif
* t = y + (1 << k);
* if (x >= t) {
* x -= t;
* q += 1 << k;
* y += 1 << (k + 1);
* }
* #if (NBITS is odd)
* x *= 2;
* #endif
* }
*
* If x0 is fixed point, rather than an integer, we can simply alter the
* scale factor between q and sqrt(x0). As it happens, we can easily arrange
* for the scale factor to be 2**0 or 1, so that sqrt(x0) == q.
*
* In our case, however, x0 (and therefore x, y, q, and t) are multiword
* integers, which adds some complication. But note that q is built one
* bit at a time, from the top down, and is not used itself in the loop
* (we use 2q as held in y instead). This means we can build our answer
* in an integer, one word at a time, which saves a bit of work. Also,
* since 1 << k is always a `new' bit in q, 1 << k and 1 << (k+1) are
* `new' bits in y and we can set them with an `or' operation rather than
* a full-blown multiword add.
*
* We are almost done, except for one snag. We must prove that none of our
* intermediate calculations can overflow. We know that x0 is in [1..4)
* and therefore the square root in q will be in [1..2), but what about x,
* y, and t?
*
* We know that y = 2q at the beginning of each loop. (The relation only
* fails temporarily while y and q are being updated.) Since q < 2, y < 4.
* The sum in t can, in our case, be as much as y+(1<<1) = y+2 < 6, and.
* Furthermore, we can prove with a bit of work that x never exceeds y by
* more than 2, so that even after doubling, 0 <= x < 8. (This is left as
* an exercise to the reader, mostly because I have become tired of working
* on this comment.)
*
* If our floating point mantissas (which are of the form 1.frac) occupy
* B+1 bits, our largest intermediary needs at most B+3 bits, or two extra.
* In fact, we want even one more bit (for a carry, to avoid compares), or
* three extra. There is a comment in fpu_emu.h reminding maintainers of
* this, so we have some justification in assuming it.
*/
struct fpn *
fpu_sqrt(struct fpemu *fe)
{
struct fpn *x = &fe->fe_f1;
u_int bit, q, tt;
u_int x0, x1, x2, x3;
u_int y0, y1, y2, y3;
u_int d0, d1, d2, d3;
int e;
FPU_DECL_CARRY;
/*
* Take care of special cases first. In order:
*
* sqrt(NaN) = NaN
* sqrt(+0) = +0
* sqrt(-0) = -0
* sqrt(x < 0) = NaN (including sqrt(-Inf))
* sqrt(+Inf) = +Inf
*
* Then all that remains are numbers with mantissas in [1..2).
*/
DPRINTF(FPE_REG, ("fpu_sqer:\n"));
DUMPFPN(FPE_REG, x);
DPRINTF(FPE_REG, ("=>\n"));
if (ISNAN(x)) {
fe->fe_cx |= FPSCR_VXSNAN;
DUMPFPN(FPE_REG, x);
return (x);
}
if (ISZERO(x)) {
fe->fe_cx |= FPSCR_ZX;
x->fp_class = FPC_INF;
DUMPFPN(FPE_REG, x);
return (x);
}
if (x->fp_sign) {
return (fpu_newnan(fe));
}
if (ISINF(x)) {
fe->fe_cx |= FPSCR_VXSQRT;
DUMPFPN(FPE_REG, 0);
return (0);
}
/*
* Calculate result exponent. As noted above, this may involve
* doubling the mantissa. We will also need to double x each
* time around the loop, so we define a macro for this here, and
* we break out the multiword mantissa.
*/
#ifdef FPU_SHL1_BY_ADD
#define DOUBLE_X { \
FPU_ADDS(x3, x3, x3); FPU_ADDCS(x2, x2, x2); \
FPU_ADDCS(x1, x1, x1); FPU_ADDC(x0, x0, x0); \
}
#else
#define DOUBLE_X { \
x0 = (x0 << 1) | (x1 >> 31); x1 = (x1 << 1) | (x2 >> 31); \
x2 = (x2 << 1) | (x3 >> 31); x3 <<= 1; \
}
#endif
#if (FP_NMANT & 1) != 0
# define ODD_DOUBLE DOUBLE_X
# define EVEN_DOUBLE /* nothing */
#else
# define ODD_DOUBLE /* nothing */
# define EVEN_DOUBLE DOUBLE_X
#endif
x0 = x->fp_mant[0];
x1 = x->fp_mant[1];
x2 = x->fp_mant[2];
x3 = x->fp_mant[3];
e = x->fp_exp;
if (e & 1) /* exponent is odd; use sqrt(2mant) */
DOUBLE_X;
/* THE FOLLOWING ASSUMES THAT RIGHT SHIFT DOES SIGN EXTENSION */
x->fp_exp = e >> 1; /* calculates (e&1 ? (e-1)/2 : e/2 */
/*
* Now calculate the mantissa root. Since x is now in [1..4),
* we know that the first trip around the loop will definitely
* set the top bit in q, so we can do that manually and start
* the loop at the next bit down instead. We must be sure to
* double x correctly while doing the `known q=1.0'.
*
* We do this one mantissa-word at a time, as noted above, to
* save work. To avoid `(1U << 31) << 1', we also do the top bit
* outside of each per-word loop.
*
* The calculation `t = y + bit' breaks down into `t0 = y0, ...,
* t3 = y3, t? |= bit' for the appropriate word. Since the bit
* is always a `new' one, this means that three of the `t?'s are
* just the corresponding `y?'; we use `#define's here for this.
* The variable `tt' holds the actual `t?' variable.
*/
/* calculate q0 */
#define t0 tt
bit = FP_1;
EVEN_DOUBLE;
/* if (x >= (t0 = y0 | bit)) { */ /* always true */
q = bit;
x0 -= bit;
y0 = bit << 1;
/* } */
ODD_DOUBLE;
while ((bit >>= 1) != 0) { /* for remaining bits in q0 */
EVEN_DOUBLE;
t0 = y0 | bit; /* t = y + bit */
if (x0 >= t0) { /* if x >= t then */
x0 -= t0; /* x -= t */
q |= bit; /* q += bit */
y0 |= bit << 1; /* y += bit << 1 */
}
ODD_DOUBLE;
}
x->fp_mant[0] = q;
#undef t0
/* calculate q1. note (y0&1)==0. */
#define t0 y0
#define t1 tt
q = 0;
y1 = 0;
bit = 1 << 31;
EVEN_DOUBLE;
t1 = bit;
FPU_SUBS(d1, x1, t1);
FPU_SUBC(d0, x0, t0); /* d = x - t */
if ((int)d0 >= 0) { /* if d >= 0 (i.e., x >= t) then */
x0 = d0, x1 = d1; /* x -= t */
q = bit; /* q += bit */
y0 |= 1; /* y += bit << 1 */
}
ODD_DOUBLE;
while ((bit >>= 1) != 0) { /* for remaining bits in q1 */
EVEN_DOUBLE; /* as before */
t1 = y1 | bit;
FPU_SUBS(d1, x1, t1);
FPU_SUBC(d0, x0, t0);
if ((int)d0 >= 0) {
x0 = d0, x1 = d1;
q |= bit;
y1 |= bit << 1;
}
ODD_DOUBLE;
}
x->fp_mant[1] = q;
#undef t1
/* calculate q2. note (y1&1)==0; y0 (aka t0) is fixed. */
#define t1 y1
#define t2 tt
q = 0;
y2 = 0;
bit = 1 << 31;
EVEN_DOUBLE;
t2 = bit;
FPU_SUBS(d2, x2, t2);
FPU_SUBCS(d1, x1, t1);
FPU_SUBC(d0, x0, t0);
if ((int)d0 >= 0) {
x0 = d0, x1 = d1, x2 = d2;
q |= bit;
y1 |= 1; /* now t1, y1 are set in concrete */
}
ODD_DOUBLE;
while ((bit >>= 1) != 0) {
EVEN_DOUBLE;
t2 = y2 | bit;
FPU_SUBS(d2, x2, t2);
FPU_SUBCS(d1, x1, t1);
FPU_SUBC(d0, x0, t0);
if ((int)d0 >= 0) {
x0 = d0, x1 = d1, x2 = d2;
q |= bit;
y2 |= bit << 1;
}
ODD_DOUBLE;
}
x->fp_mant[2] = q;
#undef t2
/* calculate q3. y0, t0, y1, t1 all fixed; y2, t2, almost done. */
#define t2 y2
#define t3 tt
q = 0;
y3 = 0;
bit = 1 << 31;
EVEN_DOUBLE;
t3 = bit;
FPU_SUBS(d3, x3, t3);
FPU_SUBCS(d2, x2, t2);
FPU_SUBCS(d1, x1, t1);
FPU_SUBC(d0, x0, t0);
ODD_DOUBLE;
if ((int)d0 >= 0) {
x0 = d0, x1 = d1, x2 = d2;
q |= bit;
y2 |= 1;
}
while ((bit >>= 1) != 0) {
EVEN_DOUBLE;
t3 = y3 | bit;
FPU_SUBS(d3, x3, t3);
FPU_SUBCS(d2, x2, t2);
FPU_SUBCS(d1, x1, t1);
FPU_SUBC(d0, x0, t0);
if ((int)d0 >= 0) {
x0 = d0, x1 = d1, x2 = d2;
q |= bit;
y3 |= bit << 1;
}
ODD_DOUBLE;
}
x->fp_mant[3] = q;
/*
* The result, which includes guard and round bits, is exact iff
* x is now zero; any nonzero bits in x represent sticky bits.
*/
x->fp_sticky = x0 | x1 | x2 | x3;
DUMPFPN(FPE_REG, x);
return (x);
}
struct fpn *
fpu_div(struct fpemu *fe)
{
struct fpn *x = &fe->fe_f1, *y = &fe->fe_f2;
u_int q, bit;
u_int r0, r1, r2, r3, d0, d1, d2, d3, y0, y1, y2, y3;
FPU_DECL_CARRY
/*
* Since divide is not commutative, we cannot just use ORDER.
* Check either operand for NaN first; if there is at least one,
* order the signalling one (if only one) onto the right, then
* return it. Otherwise we have the following cases:
*
* Inf / Inf = NaN, plus NV exception
* Inf / num = Inf [i.e., return x]
* Inf / 0 = Inf [i.e., return x]
* 0 / Inf = 0 [i.e., return x]
* 0 / num = 0 [i.e., return x]
* 0 / 0 = NaN, plus NV exception
* num / Inf = 0
* num / num = num (do the divide)
* num / 0 = Inf, plus DZ exception
*/
DPRINTF(FPE_REG, ("fpu_div:\n"));
DUMPFPN(FPE_REG, x);
DUMPFPN(FPE_REG, y);
DPRINTF(FPE_REG, ("=>\n"));
if (ISNAN(x) || ISNAN(y)) {
ORDER(x, y);
fe->fe_cx |= FPSCR_VXSNAN;
DUMPFPN(FPE_REG, y);
return (y);
}
/*
* Need to split the following out cause they generate different
* exceptions.
*/
if (ISINF(x)) {
if (x->fp_class == y->fp_class) {
fe->fe_cx |= FPSCR_VXIDI;
return (fpu_newnan(fe));
}
DUMPFPN(FPE_REG, x);
return (x);
}
if (ISZERO(x)) {
fe->fe_cx |= FPSCR_ZX;
if (x->fp_class == y->fp_class) {
fe->fe_cx |= FPSCR_VXZDZ;
return (fpu_newnan(fe));
}
DUMPFPN(FPE_REG, x);
return (x);
}
/* all results at this point use XOR of operand signs */
x->fp_sign ^= y->fp_sign;
if (ISINF(y)) {
x->fp_class = FPC_ZERO;
DUMPFPN(FPE_REG, x);
return (x);
}
if (ISZERO(y)) {
fe->fe_cx = FPSCR_ZX;
x->fp_class = FPC_INF;
DUMPFPN(FPE_REG, x);
return (x);
}
/*
* Macros for the divide. See comments at top for algorithm.
* Note that we expand R, D, and Y here.
*/
#define SUBTRACT /* D = R - Y */ \
FPU_SUBS(d3, r3, y3); FPU_SUBCS(d2, r2, y2); \
FPU_SUBCS(d1, r1, y1); FPU_SUBC(d0, r0, y0)
#define NONNEGATIVE /* D >= 0 */ \
((int)d0 >= 0)
#ifdef FPU_SHL1_BY_ADD
#define SHL1 /* R <<= 1 */ \
FPU_ADDS(r3, r3, r3); FPU_ADDCS(r2, r2, r2); \
FPU_ADDCS(r1, r1, r1); FPU_ADDC(r0, r0, r0)
#else
#define SHL1 \
r0 = (r0 << 1) | (r1 >> 31), r1 = (r1 << 1) | (r2 >> 31), \
r2 = (r2 << 1) | (r3 >> 31), r3 <<= 1
#endif
#define LOOP /* do ... while (bit >>= 1) */ \
do { \
SHL1; \
SUBTRACT; \
if (NONNEGATIVE) { \
q |= bit; \
r0 = d0, r1 = d1, r2 = d2, r3 = d3; \
} \
} while ((bit >>= 1) != 0)
#define WORD(r, i) /* calculate r->fp_mant[i] */ \
q = 0; \
bit = 1 << 31; \
LOOP; \
(x)->fp_mant[i] = q
/* Setup. Note that we put our result in x. */
r0 = x->fp_mant[0];
r1 = x->fp_mant[1];
r2 = x->fp_mant[2];
r3 = x->fp_mant[3];
y0 = y->fp_mant[0];
y1 = y->fp_mant[1];
y2 = y->fp_mant[2];
y3 = y->fp_mant[3];
bit = FP_1;
SUBTRACT;
if (NONNEGATIVE) {
x->fp_exp -= y->fp_exp;
r0 = d0, r1 = d1, r2 = d2, r3 = d3;
q = bit;
bit >>= 1;
} else {
Object *MakeExtrudeObject(PARAMS *par)
{
Object *obj;
MeshData *mesh;
MeshTri *tris = NULL, *t;
MeshVertex *v1, *v2, *pv1, *pv2, **newverts, **prevverts;
Vec3 V;
Xform *T = NULL, *Tlocal = NULL; /* Transform matrix for segments. */
int i, j, k, ntris, nverts, nsegs, reps, smooth;
double u, v;
if((mesh = Ray_NewMeshData()) == NULL)
return NULL;
/* First parameter is the number of vertices. */
nverts = (int)par->V.x;
/* Get the initial vertices... */
par = par->next;
for(i = 0; i < nverts; i++)
{
/* Get the point. */
Eval_Params(par);
if((v1 = Ray_NewMeshVertex(&par->V)) == NULL)
goto fail_create;
if(!Ray_AddMeshVertex(mesh, v1))
goto fail_create;
/* Get the optional stuff. */
if(par->more)
{
/* Normal. */
par = par->next;
v1->nx = (float)par->V.x;
v1->ny = (float)par->V.y;
v1->nz = (float)par->V.z;
v1->flags |= MESH_VERTEX_HAS_NORMAL;
if(par->more)
{
/* Color. */
par = par->next;
v1->r = (float)par->V.x;
v1->g = (float)par->V.y;
v1->b = (float)par->V.z;
v1->flags |= MESH_VERTEX_HAS_COLOR;
if(par->more)
{
/* UV coordinates. */
par = par->next;
v1->u = (float)par->V.x;
par = par->next;
v1->v = (float)par->V.x;
v1->flags |= MESH_VERTEX_HAS_UV;
}
}
}
par = par->next;
}
/*
* Get the segments.
* Create a new set of vertices as transformed copies of the
* previous set of vertices.
* Create the triangles that connect the two sets of vertices to
* form a segment.
*/
if((T = Ray_NewXform()) == NULL)
goto fail_create;
if((Tlocal = Ray_NewXform()) == NULL)
goto fail_create;
ntris = nsegs = 0;
tris = t = NULL;
smooth = 0;
while(par != NULL)
{
switch(par->type)
{
case TK_SEGMENT:
M4x4Identity(&Tlocal->M);
M4x4Identity(&Tlocal->I);
reps = 1; /* Repeat count. */
break;
case TK_SMOOTH:
smooth = 1;
break;
case TK_RIGHTBRACE: /* End of segment. */
while(reps--)
{
nsegs++;
/*
* Create a new set of vertices as transformed copies of the
* original set.
*/
ConcatXforms(T, Tlocal);
for(i = 0; i < nverts; i++)
{
if((v1 = Ray_NewMeshVertex(NULL)) == NULL)
goto fail_create;
*v1 = *mesh->vertices[i];
V.x = v1->x; V.y = v1->y; V.z = v1->z;
PointToWorld(&V, T);
v1->x = (float)V.x; v1->y = (float)V.y; v1->z = (float)V.z;
V.x = v1->nx; V.y = v1->ny; V.z = v1->nz;
if(!(ISZERO(V.x) && ISZERO(V.y) && ISZERO(V.z)))
{
NormToWorld(&V, T);
V3Normalize(&V);
v1->nx = (float)V.x; v1->ny = (float)V.y; v1->nz = (float)V.z;
}
if(!Ray_AddMeshVertex(mesh, v1))
goto fail_create;
}
/* Create connecting triangles. */
prevverts = mesh->vertices + nverts * (nsegs - 1);
newverts = mesh->vertices + nverts * nsegs;
pv1 = *prevverts++;
v1 = *newverts++;
for(i = 1; i < nverts; i++)
{
pv2 = *prevverts++;
v2 = *newverts++;
if((t = Ray_NewMeshTri(pv1, pv2, v1)) != NULL)
{
t->next = tris;
tris = t;
ntris++;
}
if((t = Ray_NewMeshTri(v2, v1, pv2)) != NULL)
{
t->next = tris;
tris = t;
ntris++;
}
pv1 = pv2;
v1 = v2;
}
/* If closed, connect the end vertices. */
}
break;
/*
* Since we know that there is always one vector parameter for all
* of the transform cases, the "TK_VECTOR" type param is
* actually the transform param, so we'll just just change
* the type to TK_VECTOR before passing it Eval_Params().
* This shortcut helps keep the parameter list short.
*/
case TK_ROTATE:
par->type = TK_VECTOR;
Eval_Params(par);
XformXforms(Tlocal, &par->V, XFORM_ROTATE);
break;
case TK_SCALE:
par->type = TK_VECTOR;
Eval_Params(par);
XformXforms(Tlocal, &par->V, XFORM_SCALE);
break;
case TK_SHEAR:
par->type = TK_VECTOR;
Eval_Params(par);
XformXforms(Tlocal, &par->V, XFORM_SHEAR);
break;
case TK_TRANSLATE:
par->type = TK_VECTOR;
Eval_Params(par);
XformXforms(Tlocal, &par->V, XFORM_TRANSLATE);
break;
case TK_REPEAT:
par->type = TK_FLOAT;
Eval_Params(par);
reps = max((int)par->V.x, 0);
break;
}
par = par->next;
}
Ray_DeleteXform(T);
Ray_DeleteXform(Tlocal);
/* If smooth, generate normals where none were explicitly defined. */
/* Generate UV coordinates where none were explicitly defined. */
k = 0;
for(i = 0; i <= nsegs; i++)
{
u = (double)i / (double)nsegs;
for(j = 0; j < nverts; j++)
{
v = (double)j / (double)(nverts - 1);
v1 = mesh->vertices[k++];
v1->u = (float)u;
v1->v = (float)v;
}
}
if((obj = Ray_MakeMeshFromData(mesh, tris, ntris)) == NULL)
goto fail_create;
if(smooth)
obj->flags |= OBJ_FLAG_SMOOTH;
return obj;
fail_create:
while(tris != NULL)
{
t = tris;
tris = t->next;
Ray_DeleteMeshTri(t);
}
Ray_DeleteMeshData(mesh);
Ray_DeleteXform(T);
Ray_DeleteXform(Tlocal);
return NULL;
}
int GDens2Eval(GDens *ptr, double *x, double *ldens, int flag)
{
int i, r;
double psum, p, xx, xlocalmax, lfmax, b, dx;
if (flag) /* sample, draw first segment then within the segment */
{
p = GMRFLib_uniform();
r = ptr->n-1;
for(i=0, psum=0.0;i<ptr->n-1;i++)
if ((psum += exp(ptr->elm[i].lp)) > p)
{
r = i; break;
}
/*
here i use a rejection sampling algorithm with two proposals: a uniform and a linear in
log-scale. pick your choice ;-)
*/
if (1)
{
/*
uniform
*/
dx = ptr->elm[r].xr-ptr->elm[r].xl;
if (ISZERO(ptr->elm[r].c))
lfmax = MAX(ptr->elm[r].lfr, ptr->elm[r].lfl) -ptr->elm[r].lfmax;
else
{
xlocalmax = -ptr->elm[r].b/(2.0*ptr->elm[r].c);
xlocalmax = MAX(0.0, xlocalmax);
xlocalmax = MIN(dx, xlocalmax);
lfmax = ptr->elm[r].c*SQR(xlocalmax)+ptr->elm[r].b*xlocalmax;
}
do
xx = GMRFLib_uniform()*dx;
while(GMRFLib_uniform() > exp(ptr->elm[r].c*SQR(xx)+ptr->elm[r].b*xx-lfmax));
*x = xx + ptr->elm[r].xl;
}
else
{
/*
linear
*/
dx = ptr->elm[r].xr -ptr->elm[r].xl;
b = (ptr->elm[r].lfr -ptr->elm[r].lfl)/dx;
if (ISZERO(ptr->elm[r].c))
{
p = GMRFLib_uniform();
xx = log(1.0-p+p*exp(b*dx))/b; /* are on a straight line */
}
else
{
xlocalmax = -(ptr->elm[r].b -b)/(2.0*ptr->elm[r].c);
if (ptr->elm[r].c < 0.0)
{
xlocalmax = MAX(0.0, xlocalmax);
xlocalmax = MIN(dx, xlocalmax);
lfmax = ptr->elm[r].c*SQR(xlocalmax)+(ptr->elm[r].b -b)*xlocalmax;
}
else
{
double lf_left, lf_right;
lf_left = 0.0;
lf_right = ptr->elm[r].c*SQR(dx)+(ptr->elm[r].b -b)*dx;
lfmax = (lf_right > lf_left ? lf_right : lf_left);
}
do
{
p = GMRFLib_uniform();
if (ISZERO(b))
xx = dx*p;
else
xx = log(1.0-p+p*exp(b*dx))/b;
}
while(GMRFLib_uniform() > exp(ptr->elm[r].c*SQR(xx)+(ptr->elm[r].b-b)*xx-lfmax));
}
*x = xx + ptr->elm[r].xl;
}
}
else /* evaluate the density, find the corresponding segment */
{
r = ptr->n-1;
for(i=0;i<ptr->n-1;i++)
if (*x >= ptr->elm[i].xl && *x < ptr->elm[i].xr)
{
r = i; break;
}
}
if (*x < ptr->xmin || *x > ptr->xmax)
{
if (0) fprintf(stdout, "\nGDens2: x=%f is not in [%f,%f], truncate!\n", *x, ptr->xmin, ptr->xmax);
xx = MAX(ptr->xmin, MIN(ptr->xmax, *x)); /* only happens in eval-mode */
}
else
{
xx = *x;
}
xx -= ptr->elm[r].xl; /* relative value within the cell */
*ldens = ptr->elm[r].lp + ptr->elm[r].c*SQR(xx) + ptr->elm[r].b*xx -ptr->elm[r].lnormc;
return 0;
}
/*
* The multiplication algorithm for normal numbers is as follows:
*
* The fraction of the product is built in the usual stepwise fashion.
* Each step consists of shifting the accumulator right one bit
* (maintaining any guard bits) and, if the next bit in y is set,
* adding the multiplicand (x) to the accumulator. Then, in any case,
* we advance one bit leftward in y. Algorithmically:
*
* A = 0;
* for (bit = 0; bit < FP_NMANT; bit++) {
* sticky |= A & 1, A >>= 1;
* if (Y & (1 << bit))
* A += X;
* }
*
* (X and Y here represent the mantissas of x and y respectively.)
* The resultant accumulator (A) is the product's mantissa. It may
* be as large as 11.11111... in binary and hence may need to be
* shifted right, but at most one bit.
*
* Since we do not have efficient multiword arithmetic, we code the
* accumulator as four separate words, just like any other mantissa.
* We use local variables in the hope that this is faster than memory.
* We keep x->fp_mant in locals for the same reason.
*
* In the algorithm above, the bits in y are inspected one at a time.
* We will pick them up 32 at a time and then deal with those 32, one
* at a time. Note, however, that we know several things about y:
*
* - the guard and round bits at the bottom are sure to be zero;
*
* - often many low bits are zero (y is often from a single or double
* precision source);
*
* - bit FP_NMANT-1 is set, and FP_1*2 fits in a word.
*
* We can also test for 32-zero-bits swiftly. In this case, the center
* part of the loop---setting sticky, shifting A, and not adding---will
* run 32 times without adding X to A. We can do a 32-bit shift faster
* by simply moving words. Since zeros are common, we optimize this case.
* Furthermore, since A is initially zero, we can omit the shift as well
* until we reach a nonzero word.
*/
struct fpn *
fpu_mul(struct fpemu *fe)
{
struct fpn *x = &fe->fe_f1, *y = &fe->fe_f2;
u_int a3, a2, a1, a0, x3, x2, x1, x0, bit, m;
int sticky;
FPU_DECL_CARRY;
/*
* Put the `heavier' operand on the right (see fpu_emu.h).
* Then we will have one of the following cases, taken in the
* following order:
*
* - y = NaN. Implied: if only one is a signalling NaN, y is.
* The result is y.
* - y = Inf. Implied: x != NaN (is 0, number, or Inf: the NaN
* case was taken care of earlier).
* If x = 0, the result is NaN. Otherwise the result
* is y, with its sign reversed if x is negative.
* - x = 0. Implied: y is 0 or number.
* The result is 0 (with XORed sign as usual).
* - other. Implied: both x and y are numbers.
* The result is x * y (XOR sign, multiply bits, add exponents).
*/
DPRINTF(FPE_REG, ("fpu_mul:\n"));
DUMPFPN(FPE_REG, x);
DUMPFPN(FPE_REG, y);
DPRINTF(FPE_REG, ("=>\n"));
ORDER(x, y);
if (ISNAN(y)) {
y->fp_sign ^= x->fp_sign;
fe->fe_cx |= FPSCR_VXSNAN;
DUMPFPN(FPE_REG, y);
return (y);
}
if (ISINF(y)) {
if (ISZERO(x)) {
fe->fe_cx |= FPSCR_VXIMZ;
return (fpu_newnan(fe));
}
y->fp_sign ^= x->fp_sign;
DUMPFPN(FPE_REG, y);
return (y);
}
if (ISZERO(x)) {
x->fp_sign ^= y->fp_sign;
DUMPFPN(FPE_REG, x);
return (x);
}
/*
* Setup. In the code below, the mask `m' will hold the current
* mantissa byte from y. The variable `bit' denotes the bit
* within m. We also define some macros to deal with everything.
*/
x3 = x->fp_mant[3];
x2 = x->fp_mant[2];
x1 = x->fp_mant[1];
x0 = x->fp_mant[0];
sticky = a3 = a2 = a1 = a0 = 0;
#define ADD /* A += X */ \
FPU_ADDS(a3, a3, x3); \
FPU_ADDCS(a2, a2, x2); \
FPU_ADDCS(a1, a1, x1); \
FPU_ADDC(a0, a0, x0)
#define SHR1 /* A >>= 1, with sticky */ \
sticky |= a3 & 1, a3 = (a3 >> 1) | (a2 << 31), \
a2 = (a2 >> 1) | (a1 << 31), a1 = (a1 >> 1) | (a0 << 31), a0 >>= 1
#define SHR32 /* A >>= 32, with sticky */ \
sticky |= a3, a3 = a2, a2 = a1, a1 = a0, a0 = 0
#define STEP /* each 1-bit step of the multiplication */ \
SHR1; if (bit & m) { ADD; }; bit <<= 1
/*
* We are ready to begin. The multiply loop runs once for each
* of the four 32-bit words. Some words, however, are special.
* As noted above, the low order bits of Y are often zero. Even
* if not, the first loop can certainly skip the guard bits.
* The last word of y has its highest 1-bit in position FP_NMANT-1,
* so we stop the loop when we move past that bit.
*/
if ((m = y->fp_mant[3]) == 0) {
/* SHR32; */ /* unneeded since A==0 */
} else {
bit = 1 << FP_NG;
do {
STEP;
} while (bit != 0);
}
if ((m = y->fp_mant[2]) == 0) {
SHR32;
} else {
bit = 1;
do {
STEP;
} while (bit != 0);
}
if ((m = y->fp_mant[1]) == 0) {
SHR32;
} else {
bit = 1;
do {
STEP;
} while (bit != 0);
}
m = y->fp_mant[0]; /* definitely != 0 */
bit = 1;
do {
STEP;
} while (bit <= m);
/*
* Done with mantissa calculation. Get exponent and handle
* 11.111...1 case, then put result in place. We reuse x since
* it already has the right class (FP_NUM).
*/
m = x->fp_exp + y->fp_exp;
if (a0 >= FP_2) {
SHR1;
m++;
}
x->fp_sign ^= y->fp_sign;
x->fp_exp = m;
x->fp_sticky = sticky;
x->fp_mant[3] = a3;
x->fp_mant[2] = a2;
x->fp_mant[1] = a1;
x->fp_mant[0] = a0;
DUMPFPN(FPE_REG, x);
return (x);
}
/*
* Perform a compare instruction (with or without unordered exception).
* This updates the fcc field in the fsr.
*
* If either operand is NaN, the result is unordered. For ordered, this
* causes an NV exception. Everything else is ordered:
* |Inf| > |numbers| > |0|.
* We already arranged for fp_class(Inf) > fp_class(numbers) > fp_class(0),
* so we get this directly. Note, however, that two zeros compare equal
* regardless of sign, while everything else depends on sign.
*
* Incidentally, two Infs of the same sign compare equal (per the 80387
* manual---it would be nice if the SPARC documentation were more
* complete).
*/
void
fpu_compare(struct fpemu *fe, int ordered)
{
struct fpn *a, *b, *r;
int cc;
a = &fe->fe_f1;
b = &fe->fe_f2;
r = &fe->fe_f3;
if (ISNAN(a) || ISNAN(b)) {
/*
* In any case, we already got an exception for signalling
* NaNs; here we may replace that one with an identical
* exception, but so what?.
*/
cc = FPSCR_FU;
if (ISSNAN(a) || ISSNAN(b))
cc |= FPSCR_VXSNAN;
if (ordered) {
if (fe->fe_fpscr & FPSCR_VE || ISQNAN(a) || ISQNAN(b))
cc |= FPSCR_VXVC;
}
goto done;
}
/*
* Must handle both-zero early to avoid sign goofs. Otherwise,
* at most one is 0, and if the signs differ we are done.
*/
if (ISZERO(a) && ISZERO(b)) {
cc = FPSCR_FE;
goto done;
}
if (a->fp_sign) { /* a < 0 (or -0) */
if (!b->fp_sign) { /* b >= 0 (or if a = -0, b > 0) */
cc = FPSCR_FL;
goto done;
}
} else { /* a > 0 (or +0) */
if (b->fp_sign) { /* b <= -0 (or if a = +0, b < 0) */
cc = FPSCR_FG;
goto done;
}
}
/*
* Now the signs are the same (but may both be negative). All
* we have left are these cases:
*
* |a| < |b| [classes or values differ]
* |a| > |b| [classes or values differ]
* |a| == |b| [classes and values identical]
*
* We define `diff' here to expand these as:
*
* |a| < |b|, a,b >= 0: a < b => FSR_CC_LT
* |a| < |b|, a,b < 0: a > b => FSR_CC_GT
* |a| > |b|, a,b >= 0: a > b => FSR_CC_GT
* |a| > |b|, a,b < 0: a < b => FSR_CC_LT
*/
#define opposite_cc(cc) ((cc) == FPSCR_FL ? FPSCR_FG : FPSCR_FL)
#define diff(magnitude) (a->fp_sign ? opposite_cc(magnitude) : (magnitude))
if (a->fp_class < b->fp_class) { /* |a| < |b| */
cc = diff(FPSCR_FL);
goto done;
}
if (a->fp_class > b->fp_class) { /* |a| > |b| */
cc = diff(FPSCR_FG);
goto done;
}
/* now none can be 0: only Inf and numbers remain */
if (ISINF(a)) { /* |Inf| = |Inf| */
cc = FPSCR_FE;
goto done;
}
fpu_sub(fe);
if (ISZERO(r))
cc = FPSCR_FE;
else if (r->fp_sign)
cc = FPSCR_FL;
else
cc = FPSCR_FG;
done:
fe->fe_cx = cc;
}
GDens *GDens2InitNew(double fac, double mean, double stdev, int n, GDensFunc_tp *lfunc, char *lfunc_arg)
{
/*
arguments are:
limits are mean +- fac*stdev, and n-regions.
lfunc: the function returning log(f(x,lfunc_arg)), unormalized
*/
GDens *ptr;
int i;
double dx, df, f, *cpoints;
fac = ABS(fac);
ptr = Calloc(1, GDens);
ptr->xmin = mean -fac*stdev;
ptr->xmax = mean +fac*stdev;
ptr->n = n;
ptr->elm = Calloc(n+1, GDensElm);
cpoints = cut_points(fac, n);
for(i=0;i<ptr->n;i++)
{
dx = stdev*(cpoints[i+1]-cpoints[i]);
ptr->elm[i].xl = mean + cpoints[i]*stdev;
ptr->elm[i].xr = ptr->elm[i].xl + dx;
ptr->elm[i].lfl = (i==0 ? lfunc(ptr->elm[i].xl, lfunc_arg) : ptr->elm[i-1].lfr);
ptr->elm[i].lfr = lfunc(ptr->elm[i].xr, lfunc_arg);
/*
now, try to decide a nice place to place the midpoint. currently, i try to split the
region into (approximately) equal integrals
*/
df = ptr->elm[i].lfr - ptr->elm[i].lfl;
f = (ISZERO(df) ? 0.5 : log(0.5*(exp(df)+1.0))/df);
/*
need also a more safe option than ISZERO
*/
if (ISZERO(f) || ISZERO(1.-f)) f = 0.5;
if (f < FLT_EPSILON || 1.-f < FLT_EPSILON) f = 0.5;
if (f == 0.5)
{
/*
this will emulate a log-linear spline
*/
ptr->elm[i].xm = ptr->elm[i].xl + dx*f;
ptr->elm[i].lfm = 0.5*(ptr->elm[i].lfl + ptr->elm[i].lfr);
}
else
{
ptr->elm[i].xm = ptr->elm[i].xl + dx*f;
ptr->elm[i].lfm = lfunc(ptr->elm[i].xm, lfunc_arg);
}
}
for(i=0;i<ptr->n-1;i++) ptr->elm[i].lfr = ptr->elm[i+1].lfl;
ptr->elm[ptr->n-1].lfr = lfunc(ptr->elm[ptr->n-1].xr, lfunc_arg);
GDens2Update(ptr);
return ptr;
}
GDens *GDens2Init(double xmin, double xmax, int n, int n_min, GDensFunc_tp *lfunc, char *lfunc_arg)
{
/*
arguments are:
xmin : left limit
xmax : right limit
n : total number of interior points, which makes n+1 cells
n_min: make first n_min interior points by equal division of (xmax-xmin), then
n-n_min divisions by dividing the cell with the largest probability in half.
lfunc: the function returning log(f(x,lfunc_arg)), unormalized
*/
GDens *ptr;
int i, nn;
double dx, df, f;
ptr = Calloc(1, GDens);
ptr->order= 2;
ptr->xmin = MIN(xmin, xmax);
ptr->xmax = MAX(xmin, xmax);
ptr->n = MAX(1, n_min);
nn = MAX(ptr->n, n);
ptr->elm = Calloc(nn+1, GDensElm);
/*
make first equal spaced points
*/
dx = (ptr->xmax -ptr->xmin)/((double)n_min);
for(i=0;i<ptr->n;i++)
{
ptr->elm[i].xl = ptr->xmin + i*dx;
ptr->elm[i].xr = ptr->elm[i].xl + dx;
ptr->elm[i].lfl = (i==0 ? lfunc(ptr->elm[i].xl, lfunc_arg) : ptr->elm[i-1].lfr);
ptr->elm[i].lfr = lfunc(ptr->elm[i].xr, lfunc_arg);
/*
now, try to decide a nice place to place the midpoint. currently, i try to split the
region into (approximately) equal integrals
*/
df = ptr->elm[i].lfr - ptr->elm[i].lfl;
f = (ISZERO(df) ? 0.5 : log(0.5*(exp(df)+1.0))/df);
/*
need also a more safe option than ISZERO
*/
if (ISZERO(f) || ISZERO(1.-f)) f = 0.5;
if (f < FLT_EPSILON || 1.-f < FLT_EPSILON) f = 0.5;
if (f == 0.5)
{
/*
this will emulate a log-linear spline
*/
ptr->elm[i].xm = ptr->elm[i].xl + dx*f;
ptr->elm[i].lfm = 0.5*(ptr->elm[i].lfl + ptr->elm[i].lfr);
}
else
{
ptr->elm[i].xm = ptr->elm[i].xl + dx*f;
ptr->elm[i].lfm = lfunc(ptr->elm[i].xm, lfunc_arg);
}
}
for(i=0;i<ptr->n-1;i++) ptr->elm[i].lfr = ptr->elm[i+1].lfl;
ptr->elm[ptr->n-1].lfr = lfunc(ptr->elm[ptr->n-1].xr, lfunc_arg);
GDens2Update(ptr);
/*
decide now the rest of the points by dividing the cell with largest probability in half. try
to split the region into (approximately) equal integrals
*/
for(i=n_min;i<nn;i++)
{
/*
split the first elm, and add one at the end
*/
ptr->elm[ptr->n].lfl = ptr->elm[0].lfm;
ptr->elm[ptr->n].xl = ptr->elm[0].xm;
ptr->elm[ptr->n].lfr = ptr->elm[0].lfr;
ptr->elm[ptr->n].xr = ptr->elm[0].xr;
df = ptr->elm[ptr->n].lfr - ptr->elm[ptr->n].lfl;
f = (ISZERO(df) ? 0.5 : log(0.5*(exp(df)+1.0))/df);
if (ISZERO(f) || ISZERO(1.-f)) f = 0.5;
if (f < FLT_EPSILON || 1.-f < FLT_EPSILON) f = 0.5;
if (f == 0.5)
{
/*
this will emulate a log-linear spline
*/
ptr->elm[ptr->n].xm = ptr->elm[ptr->n].xl + f*(ptr->elm[ptr->n].xr -ptr->elm[ptr->n].xl);
ptr->elm[ptr->n].lfm = 0.5*(ptr->elm[ptr->n].lfl + ptr->elm[ptr->n].lfr);
}
else
{
ptr->elm[ptr->n].xm = ptr->elm[ptr->n].xl + f*(ptr->elm[ptr->n].xr -ptr->elm[ptr->n].xl);
ptr->elm[ptr->n].lfm = lfunc(ptr->elm[ptr->n].xm, lfunc_arg);
}
ptr->elm[0].lfr = ptr->elm[0].lfm;
ptr->elm[0].xr = ptr->elm[0].xm;
df = ptr->elm[0].lfr - ptr->elm[0].lfl;
f = (ISZERO(df) ? 0.5 : log(0.5*(exp(df)+1.0))/df);
if (ISZERO(f) || ISZERO(1.-f)) f = 0.5;
if (f < FLT_EPSILON || 1.-f < FLT_EPSILON) f = 0.5;
if (f == 0.5)
{
/*
this will emulate a log-linear spline
*/
ptr->elm[0].xm = ptr->elm[0].xl + f*(ptr->elm[0].xr -ptr->elm[0].xl);
ptr->elm[0].lfm = 0.5*(ptr->elm[0].lfl + ptr->elm[0].lfr);
}
else
{
ptr->elm[0].xm = ptr->elm[0].xl + f*(ptr->elm[0].xr -ptr->elm[0].xl);
ptr->elm[0].lfm = lfunc(ptr->elm[0].xm, lfunc_arg);
}
ptr->n++;
GDens2Update(ptr);
}
return ptr;
}
int GDens2Update(GDens *ptr)
{
/*
update ptr
*/
double lpmax, lpsum, psum;
int i, n = ptr->n;
/*
compute various constants
*/
for(i=0;i<n;i++)
{
double s,
x0 = ptr->elm[i].xl, x1 = ptr->elm[i].xm, x2 = ptr->elm[i].xr,
f0 = ptr->elm[i].lfl, f1 = ptr->elm[i].lfm, f2 = ptr->elm[i].lfr;
ptr->elm[i].lfmax = f0; /* scale and shift */
f1 -= f0; f2 -= f0; f0 = 0.0;
x1 -= x0; x2 -= x0; x0 = 0.0;
s = 1./((-SQR(x2) + x2*x1)*x1);
ptr->elm[i].b = -s*(-f2*SQR(x1) +f1*SQR(x2));
ptr->elm[i].c = s*(-f2*x1 +f1*x2);
}
/*
compute the integral over each cell, unnormalized
*/
for(i=0;i<n;i++)
{
if (ISZERO(ptr->elm[i].c))
{
if (ISZERO(ptr->elm[i].b))
ptr->elm[i].lnormc = log(ptr->elm[i].xr-ptr->elm[i].xl);
else
{
double tmp = (ptr->elm[i].xr-ptr->elm[i].xl)*ptr->elm[i].b;
if (fabs(tmp) > FLT_EPSILON)
{
/*
all ok
*/
ptr->elm[i].lnormc = log(fabs(exp((ptr->elm[i].xr-ptr->elm[i].xl)*ptr->elm[i].b) -1.0))
- log(fabs(ptr->elm[i].b));
}
else
{
/*
this is the expansion for small 'tmp'
*/
ptr->elm[i].lnormc = log(fabs(tmp)) - log(fabs(ptr->elm[i].b));
}
}
}
else
{
/*
this can (and does) go wrong as the 'erf' and 'erfi' function may fail for high
values of the arguments. perhaps we need to intergrate directly
int(exp(x^2),x=low...high) ?
*/
double aa = 0.5*ptr->elm[i].b/sqrt(fabs(ptr->elm[i].c));
if (ptr->elm[i].c <= 0.0)
ptr->elm[i].lnormc = -0.25*SQR(ptr->elm[i].b)/ptr->elm[i].c +
log(1./(TWODIVSQRTPI*sqrt(-ptr->elm[i].c)))
+ lerf_diff(sqrt(-ptr->elm[i].c)*(ptr->elm[i].xr-ptr->elm[i].xl) -aa, -aa);
else
ptr->elm[i].lnormc = -0.25*SQR(ptr->elm[i].b)/ptr->elm[i].c +
log(1./(TWODIVSQRTPI*sqrt(ptr->elm[i].c)))
+ lerfi_diff(sqrt(ptr->elm[i].c)*(ptr->elm[i].xr-ptr->elm[i].xl) +aa, aa);
}
if (ISINF(ptr->elm[i].lnormc))
{
ptr->elm[i].lnormc = ptr->elm[i].b = ptr->elm[i].c = 0;
ptr->elm[i].lp = ptr->elm[i].lfl = ptr->elm[i].lfm = ptr->elm[i].lfr = ptr->elm[i].lfmax = -FLT_MAX;
}
else
{
ptr->elm[i].lp = ptr->elm[i].lnormc + ptr->elm[i].lfmax;
}
}
for(lpmax = ptr->elm[0].lp, i=1;i<n;i++) lpmax = MAX(lpmax, ptr->elm[i].lp);
for(i=0;i<n;i++) ptr->elm[i].lp -= lpmax;
for(i=0, psum=0.0;i<n;i++) psum += exp(ptr->elm[i].lp);
for(lpsum = log(psum), i=0;i<n;i++) ptr->elm[i].lp -= lpsum;
/*
speedup the search for regions and sampling
*/
qsort(ptr->elm, (size_t) ptr->n, sizeof(GDensElm), GDensElmCompare);
if (0)
{
for(i=0;i<n;i++)
{
printf("Gdens2 region %d\n", i);
printf("\txl %f xr %f\n", ptr->elm[i].xl, ptr->elm[i].xr);
printf("\tlnormc %f lp %f\n", ptr->elm[i].lnormc, ptr->elm[i].lp);
}
}
return 0;
}
/*****************************************************************************
Function Name : tcc_composite_process()
******************************************************************************/
void tcc_composite_process(struct tcc_lcdc_image_update *update, char force_update)
{
COMPOSITE_LCDC_IMG_CTRL_TYPE ImgCtrl;
unsigned int Y_offset, UV_offset;
unsigned int img_width = 0, img_height = 0, win_offset_x = 0, win_offset_y = 0;
unsigned int lcd_w = 0, lcd_h =0;
unsigned int lcd_ctrl_flag, mixer_interrupt_use = 0;
exclusive_ui_ar_params aspect_ratio;
PLCDC pLCDC;
if(Composite_LCDC_Num)
pLCDC = (volatile PLCDC)tcc_p2v(HwLCDC1_BASE);
else
pLCDC = (volatile PLCDC)tcc_p2v(HwLCDC0_BASE);
tcc_composite_get_lcdsize(&lcd_w, &lcd_h);
img_width = update->Image_width;
img_height = update->Image_height;
dprintk("%s, img_w=%d, img_h=%d, lcd_w=%d, lcd_h=%d, plane_w=%d, plane_ht=%d\n", __func__,
img_width, img_height, lcd_w, lcd_h, composite_exclusive_ui_param.plane_width, composite_exclusive_ui_param.plane_height);
/* Aspect Ratio Conversion for DVD Contents */
//if((img_width <= COMPOSITE_EXCLUSIVE_UI_DVD_MAX_WD) && (img_height <= COMPOSITE_EXCLUSIVE_UI_DVD_MAX_HT))
{
memset((void *)&aspect_ratio, 0x00, sizeof(exclusive_ui_ar_params));
TCC_OUTPUT_EXCLUSIVE_UI_CfgAR(Composite_LCDC_Num, img_width, img_height, &aspect_ratio);
}
/* Get the parameters for exclusive ui */
TCC_OUTPUT_EXCLUSIVE_UI_GetParam(&composite_exclusive_ui_param);
if(composite_exclusive_ui_param.enable)
{
/* Clear the on_the_flay for video */
composite_exclusive_ui_onthefly = FALSE;
/* Clear update flag and backup an information of the last video */
TCC_OUTPUT_EXCLUSIVE_UI_SetImage(update);
/*-------------------------------------*/
/* INTERLACE VIDEO */
/* Update video data with exclusive ui */
/*-------------------------------------*/
if(composite_exclusive_ui_param.interlace)
{
/* Interlace BD Contents with 1920x1080 graphic plane */
if(!TCC_OUTPUT_EXCLUSIVE_UI_ViqeMtoM(Composite_LCDC_Num, img_width, img_height))
{
if((img_width == lcd_w) && (img_height == lcd_h))
{
/* 1920x1080 COMPOSITE Output */
}
else
{
/* 1280x720 COMPOSITE Output */
}
}
else
{
/* Disable the lcdc channel_1 */
TCC_OUTPUT_EXCLUSIVE_UI_Direct(Composite_LCDC_Num, FALSE);
/* VIQE and M2M Scaler with On_the_fly */
/* Check the output format */
if(ISZERO(pLCDC->LCTRL, HwLCTRL_NI))
lcd_h = lcd_h >> 1;
if(composite_exclusive_ui_param.clear == TRUE)
{
/* M2M Scaler without On_the_fly */
if(TCC_OUTPUT_EXCLUSIVE_UI_Scaler_OnTheFly(update, Composite_LCDC_Num) < 0)
composite_exclusive_ui_onthefly = FALSE;
else
composite_exclusive_ui_onthefly = TRUE;
/* Update Screen */
tcc_composite_update(update);
}
else
{
TCC_OUTPUT_EXCLUSIVE_UI_Scaler(update, COMPOSITE_EXCLUSIVE_UI_SCALER1, composite_exclusive_ui_param.plane_width, composite_exclusive_ui_param.plane_height, composite_exclusive_ui_idx, 0);
if(TCC_OUTPUT_EXCLUSIVE_UI_GetUpdate())
{
dprintk("E\n");
}
else
{
/* Overlay Mixer */
TCC_OUTPUT_EXCLUSIVE_UI_Mixer(update, composite_exclusive_ui_idx, 0);
/* Update overlay mixer image */
TCC_OUTPUT_EXCLUSIVE_UI_Restore_Mixer(update, Composite_LCDC_Num, lcd_w, lcd_h, composite_exclusive_ui_idx);
/* Set the interrupt flag */
mixer_interrupt_use = 1;
}
}
}
}
else
/*-------------------------------------*/
/* PROGRESSIVE VIDEO */
/* Update video data with exclusive ui */
/*-------------------------------------*/
{
if( (img_width == composite_exclusive_ui_param.plane_width) && (img_width == lcd_w) && (img_width == TCC_FB_OUT_MAX_WIDTH) &&