static void apply(Alpha a, const V1 &x, const V2 &y, Beta b, V3 &z)
{
if (!math::is_zero(b))
z.array() = a * x.array() * y.array() + b * z.array();
else
z.array() = a * x.array() * y.array();
}
void frenet(const V3& d1, const V3& d2, V3& t, V3& n, V3& b){
b = cross(d2, d1);
n = cross(d1, b);
t = d1 * 1./sqrt((d1.magSqr()));
b *= 1./sqrt(b.magSqr());
n *= 1./sqrt(n.magSqr());
}
void Canopy::growHexSegment(treeNode *rootOfTree, BranchBase *pCanopyBranch,aabb TreeBoundingBox, LevelDetail *grammar, V3 startHeading)
{
theOverseer = observer::Instance();
rootOfTree->tree->m_CanopyCount++;
V3 root(rootOfTree->pbranch->segments[0].m_tipPointList[0]);
V3 CanopySegmentRoot(pCanopyBranch->tipPoint);// canopySegmentRoot
V3 CanopyTop(root);
CanopyTop.y = root.y + TreeBoundingBox.yMax;
V3 CanopyFulcrum(CanopyTop);
CanopyFulcrum.y = CanopyFulcrum.y * 0.57f; // 4/7
if(pCanopyBranch->tipPoint.y < CanopyFulcrum.y){
CanopyFulcrum.y = pCanopyBranch->tipPoint.y * 0.714f;// 5/7
}
V3 CanopyHeading = CanopySegmentRoot-CanopyFulcrum;
CanopyHeading.Normalize();
V3 CanopyArbitrary(CanopySegmentRoot-root);
CanopyArbitrary.Normalize();
V3 Left = CrossProduct(CanopyArbitrary,CanopyHeading);
V3 Right = CrossProduct(CanopyHeading,CanopyArbitrary);
Left.Normalize();
Right.Normalize();
V3 Down = CrossProduct(Left,CanopyHeading);
V3 Up = CrossProduct(CanopyHeading,Left);
V3 perturb = CanopyHeading;
Down.Normalize();
Up.Normalize();
int shift = -((int)floor(m_width/2.0f));
float nudge =0.0f;
growPatchSegment(rootOfTree, pCanopyBranch,TreeBoundingBox, grammar,startHeading);
}
// Professor's implemetation is more elegant and compact
V3 V3::rotateThisPointAboutAxis(V3 Oa, V3 aDir, float theta) {
// build aux coordinate system with axis as one of its principal axes
V3 auxAxis;
if (fabsf(aDir[0]) > fabsf(aDir[1])) {
auxAxis = V3(0.0f, 1.0f, 0.0f);
}
else {
auxAxis = V3(1.0f, 0.0f, 0.0f);
}
V3 yl = aDir;
V3 zl = (aDir ^ auxAxis);
zl.normalize();
V3 xl = (yl ^ zl);
xl.normalize();
M33 lcs;
lcs[0] = xl;
lcs[1] = yl;
lcs[2] = zl;
// transform to aux coordinate system
V3 &p = *this;
V3 p1 = lcs*(p - Oa);
// rotate about principal axis
M33 roty;
roty.setRotationAboutY(theta);
V3 p2 = roty * p1;
// transform back to old world
V3 ret = lcs.getInverted()*p2 + Oa;
return ret;
}
Matrix<POSE_T, 3, 1> find_opt_cc(NormalAOPoseAdapter<POSE_T, POINT_T>& adapter)
{
//the R has been fixed, we need to find optimal cc, camera center, given n pairs of 2-3 correspondences
//Slabaugh, G., Schafer, R., & Livingston, M. (2001). Optimal Ray Intersection For Computing 3D Points From N -View Correspondences.
typedef Matrix<POSE_T, 3, 1> V3;
typedef Matrix<POSE_T, 3, 3> M3;
M3 Rwc = adapter.getRcw().inverse().matrix();
M3 AA; AA.setZero();
V3 bb; bb.setZero();
for (int i = 0; i < adapter.getNumberCorrespondences(); i++)
{
if (adapter.isInlier23(i)){
V3 vr_w = Rwc * adapter.getBearingVector(i).template cast<POSE_T>();
M3 A;
A(0,0) = 1 - vr_w(0)*vr_w(0);
A(1,0) = A(0,1) = - vr_w(0)*vr_w(1);
A(2,0) = A(0,2) = - vr_w(0)*vr_w(2);
A(1,1) = 1 - vr_w(1)*vr_w(1);
A(2,1) = A(1,2) = - vr_w(1)*vr_w(2);
A(2,2) = 1 - vr_w(2)*vr_w(2);
V3 b = A * adapter.getPointGlob(i).template cast<POSE_T>();
AA += A;
bb += b;
}
}
V3 c_w;
if (fabs(AA.determinant()) < POSE_T(0.0001))
c_w = V3(numeric_limits<POSE_T>::quiet_NaN(), numeric_limits<POSE_T>::quiet_NaN(), numeric_limits<POSE_T>::quiet_NaN());
else
c_w = AA.jacobiSvd(ComputeFullU | ComputeFullV).solve(bb);
return c_w;
}
void PPC::positionRelativeToPoint(
const V3 & P,
const V3 & vd,
const V3 & up,
float distance)
{
// assumes: up and vd are normalized
// quit early if assumptions are not met
if (!((fabs(vd.length() - 1.0f)) < epsilonNormalizedError) ||
!((fabs(up.length() - 1.0f)) < epsilonNormalizedError)) {
cerr << "ERROR: Up or vd vectors are not normal vectors. Camera positioning aborted..." << endl;
return;
}
V3 newa, newb, newc, newC;
// compute new C, a, and b
newC = P - (vd * distance);
newa = (vd ^ up).getNormalized() * a.length();
newb = (vd ^ newa).getNormalized() * b.length();
V3 principalPoint = getPrincipalPoint();
float PPu = principalPoint.getX();
float PPv = principalPoint.getY();
// compute new c
newc = vd*getFocalLength() - (PPu * newa) - (PPv * newb);
// commit new values
C = newC;
a = newa;
b = newb;
c = newc;
// update projection matrix
buildProjM();
}
double angle(const V3 & u, const V3 & v)
{
double dot = u * v;
double nu = u.normL2();
double nv = v.normL2();
return remainder(acos(dot/(nu*nv)),2*M_PI);
}
V3 bounce(Ray ray, V3 normal) {
// cout << "chrome bounce\n";
double theta1 = fabs(ray.direction.dot(normal));
double internalIndex, externalIndex;
if (theta1 >= 0.0L) {
internalIndex = ior;
externalIndex = 1.0L;
} else {
internalIndex = 1.0L;
externalIndex = ior;
}
double eta = externalIndex/internalIndex;
double theta2 = sqrt(1.0L - (eta * eta) * (1.0L - (theta1 * theta1)));
double rs = (externalIndex * theta1 - internalIndex * theta2) / (externalIndex*theta1 + internalIndex * theta2);
double rp = (internalIndex * theta1 - externalIndex * theta2) / (internalIndex*theta1 + externalIndex * theta2);
double reflectance = (rs*rs + rp*rp);
//reflection
if(unifRand() < reflectance+reflection) {
return ray.direction.add(normal.muls(theta1*2.0L));
}
// refraction
return (ray.direction.add(normal.muls(theta1)).muls(eta) \
.add(normal.muls(-theta2)));
}
void PPC::setByInterpolation(PPC &ppc0, PPC &ppc1, int i, int n)
{
// assumption is that ppc0 and ppc1 have the same internal parameters
float t = (float)i / (float)(n - 1);
// Ci
C = ppc0.C + (ppc1.C - ppc0.C) * t;
// vdi
V3 vd0 = ppc0.getViewDir();
V3 vd1 = ppc1.getViewDir();
V3 vdi = vd0 + (vd1 - vd0) * t;
// ai
a = ppc0.a + (ppc1.a - ppc0.a) * t;
// in order to calculate b and c we use the camera positioning
// formulation. These formulas require us to know internal
// camera parameters such as PPu, PPv, f, a.length and b.length.
// But since we are assuming both endpoint cameras have the same
// internals we can use camera zero as reference
float PPu = ppc0.getPrincipalPoint().getX();
float PPv = ppc0.getPrincipalPoint().getY();
float f = ppc0.getFocalLength();
// we could assume this is always one in our case but are calculated
// here anyways just to follow the slides which apply for more general cameras
float bLength = ppc0.b.length();
// bi
V3 orthoNormalVector = vdi ^ a;
orthoNormalVector.normalize();
b = orthoNormalVector * bLength;
// ci
c = -1.0f * (PPu * a) - (PPv * b) +
vdi*f;
// update projection matrix
buildProjM();
}
inline void bi::exp_vector(V2 x, const V3& is) {
BOOST_AUTO(iter, is.begin());
BOOST_AUTO(end, is.end());
for (; iter != end; ++iter) {
BOOST_AUTO(elem, subrange(x, *iter, 1));
exp_elements(elem, elem);
}
}
inline void frenet(const V3& d1, const V3& d2, V3& t, V3& n, V3& b){
b = cross(d2, d1);
n = cross(d1, b);
t = d1;
t.normalize();
b.normalize();
n.normalize();
}
V5() {
thunk_OK = this;
if( bar() < 1000 ) {
V2 *p = this;
p->foo( bar() );
V3 *q = this;
q->foo( bar() );
}
}
/**
* Draw axis aligned rectangle.
*
* @param p0 the top left corner of the rectangle
* @param p1 the bottom right corner of the rectangle
* @param c the color
*/
void FrameBuffer::DrawRectangle(const V3 &p0, const V3 &p1, const V3 &c) {
V3 p2(p0.x(), p1.y(), 0.0f);
V3 p3(p1.x(), p0.y(), 0.0f);
Draw2DSegment(p0, c, p2, c);
Draw2DSegment(p2, c, p1, c);
Draw2DSegment(p1, c, p3, c);
Draw2DSegment(p3, c, p0, c);
}
Camera(V3 iorigin, V3 itopleft, V3 itopright, V3 ibottomleft) {
origin = iorigin;
topleft = itopleft;
topright = itopright;
bottomleft = ibottomleft;
xd = topright.sub(topleft);
yd = bottomleft.sub(topleft);
}
real bi::det_vector(const V2 x, const V3& is) {
BOOST_AUTO(iter, is.begin());
BOOST_AUTO(end, is.end());
real det = 1.0;
for (; iter != end; ++iter) {
det *= *(x.begin() + *iter);
}
return det;
}
/**
* Setup this camera such that it looks at the specified point with the specified view direction, up direction, and the distance from the point.
*
* @param p the target point that this camera will look at
* @param vd the view direction
* @param up the up direction
* @param d the distance between the center of this camera and the target
*/
void PPC::LookAt(const V3 &p, const V3 &vd, const V3 &up, float d) {
float f = GetFocalLength();
C = p - vd.UnitVector() * d;
a = (vd ^ up).UnitVector() * a.Length();
b = (vd ^ a).UnitVector() * b.Length();
c = vd.UnitVector() * f - a * ((float)w / 2.0f) - b * ((float)h / 2.0f);
SetPMat();
}
V3 PPC::unproject(const V3 & projP) const
{
// From projection of point formula we know
// P = C + (au + bv + c) * w
// but since our project function above returns 1/w
// P = C + (au + bv + c) * (1/w)
V3 ret = C + (a*projP.getX() + b*projP.getY() + c) / projP.getZ();
return ret;
}
double intersect(Ray r) {
double n_dot_u = normal.dot(r.direction);
if ((n_dot_u > -0.00001L) && (n_dot_u < 0.00001L)) {
return -1.0L;
}
double n_dot_p0 = normal.dot(center.sub(r.origin));
return n_dot_p0 / n_dot_u;
}
double intersect(Ray r) {
// cout << "Sphere intersect()" << endl;
V3 distance = r.origin.sub(center);
double b = distance.dot(r.direction);
double c = distance.dot(distance) - radius2;
double d = (b * b) - c;
if (d > 0.0L) {
return -b - sqrt(d);
} else {
return -1.0L;
}
}
void format_type(Formatter &f, const V3 &v)
{
string fmt = f.front();
if(fmt == "%(abs)") { f.out << abs(v); }
else if(fmt == "%(polar())") {
f.out << io::format("[r=%f theta=%f phi=%f]") << abs(v) << deg(v.theta()) << deg(v.phi());
}
else if(fmt == "%(x)") { f.out << v.x; }
else if(fmt == "%(y)") { f.out << v.y; }
else if(fmt == "%(z)") { f.out << v.z; }
else f.out << v;
}
/**
* Scale this mesh by placing the centroid at givin position and scaling to given AABB size.
*
* @param centroid the given position for the new centroid
* @param size the given AABB size
*/
void TMesh::Scale(const V3 ¢roid, const V3 &size) {
AABB aabb;
ComputeAABB(aabb);
V3 c = GetCentroid();
V3 scale(size.x() / aabb.Size().x(), size.y() / aabb.Size().y(), size.z() / aabb.Size().z());
for (int i = 0; i < vertsN; i++) {
verts[i].v[0] = (verts[i].v.x() - c.x()) * scale.x() + centroid.x();
verts[i].v[1] = (verts[i].v.y() - c.y()) * scale.y() + centroid.y();
verts[i].v[2] = (verts[i].v.z() - c.z()) * scale.z() + centroid.z();
}
}
void PPC::zoom(float zoomFactor)
{
// calculate new focal length
float newFocalLength = getFocalLength() * zoomFactor;
// calculate new c
V3 principalPoint = getPrincipalPoint();
V3 viewDirection = getViewDir();
float PPu = principalPoint.getX();
float PPv = principalPoint.getY();
c = -1.0f * (PPu * a) - (PPv * b) +
viewDirection*newFocalLength;
// update projection matrix
buildProjM();
}
V3 PPC::getPrincipalPoint(void) const
{
// this is a general formula that handles the case
// when the camera does not have a square pixel of unit length
// (a and b are not magnitude 1)
// create copies here to honor const
V3 aNormalized = a;
V3 bNormalized = b;
// again in case the pixel is not square unit length
aNormalized.normalize();
bNormalized.normalize();
float PPu = -1.0f * (c * aNormalized) / a.length();
float PPv = -1.0f * (c * bNormalized) / b.length();
return V3(PPu, PPv, 0.0f);
}
void FrameBuffer::Draw3DPoint(V3 pt, PPC *ppc, int psize, V3 color) {
V3 ppt;
if (!ppc->Project(pt, ppt))
return;
DrawPoint((int)ppt[0], (int)ppt[1], psize, color.getColor());
}
void FrameBuffer::Draw2DBigPoint(int u0, int v0, int psize, const V3 &color, float z) {
for (int v = v0-psize/2; v <= v0+psize/2; v++) {
for (int u = u0-psize/2; u <= u0+psize/2; u++) {
SetGuarded(u, v, color.GetColor(), z);
}
}
}
void FireBall::initParticle(ParticleEmitter::Particle& out)
{
// Time particle is created relative to the global running
// time of the particle system.
out.initialTime = mTime;
out.age=mTime;
// Flare lives for 2-4 seconds.
// original values //out.lifeTime = GetRandomFloat(2.0f, 8.0f);
out.lifeTime = GetRandomFloat(mMinLifeTime, mMaxLifeTime);
// Initial size in pixels.
// original values //out.initialSize = GetRandomFloat(10.0f, 15.0f);
out.initialSize = GetRandomFloat(mMinSize, mMaxSize);
// Give a very small initial velocity to give the flares
// some randomness.
GetRandomVec(out.initialVelocity);
out.initialVelocity.x *= mAccelImpulse.x;
out.initialVelocity.y *= mAccelImpulse.y;
out.initialVelocity.z *= mAccelImpulse.z;
out.initialVelocity.x += mAccelShift.x;
out.initialVelocity.y += mAccelShift.y;
out.initialVelocity.z += mAccelShift.z;
// Scalar value used in vertex shader as an amplitude factor.
out.mass = GetRandomFloat(1.0f, 2.0f);
// Start color at 50-100% intensity when born for variation.
out.initialColor = static_cast<DWORD>(GetRandomFloat(0.5f, 1.0f) * 1.0f);//white;
V3 temp = mParticleRadius - mInitPos;
float length = temp.Length();
V3 tempNormal = temp.Normalize();
//D3DXMATRIX outmtx;
noMat3 outmtx;
//D3DXMatrixRotationYawPitchRoll(&outmtx,GetRandomFloat(0.0f,2.0f)*noMath::PI,GetRandomFloat(0.0f,2.0f)*noMath::PI,0.0f);
noAngles angle(RAD2DEG(GetRandomFloat(0.0f,2.0f)*noMath::PI), RAD2DEG(GetRandomFloat(0.0f,2.0f)*noMath::PI), 0.0f);
outmtx = angle.ToMat3();
//D3DXVec3TransformCoord(&temp,&temp,&outmtx);
temp = outmtx * temp;
out.initialPos = mInitPos + (temp * length);
}
void bi::hist(const V1 x, const V2 w, V3 c, V4 h) {
/* pre-condition */
BI_ASSERT(x.size() == w.size());
BI_ASSERT(c.size() == h.size());
BI_ASSERT(!V3::on_device);
BI_ASSERT(!V4::on_device);
typedef typename V1::value_type T1;
typedef typename V2::value_type T2;
const int P = x.size();
const int B = c.size();
T1 mx, mn;
int i, j;
typename temp_host_vector<T1>::type xSorted(P);
typename temp_host_vector<T2>::type wSorted(P);
xSorted = x;
wSorted = w;
bi::sort_by_key(xSorted, wSorted);
mn = xSorted[0];
mx = xSorted[xSorted.size() - 1];
/* compute bin right edges */
for (j = 0; j < B; ++j) {
c[j] = mn + (j + 1)*(mx - mn)/B;
}
/* compute bin heights */
h.clear();
for (i = 0, j = 0; i < P; ++i) {
if (xSorted[i] >= c[j] && j < B - 1) {
++j;
}
h[j] += wSorted[i];
}
/* compute bin centres */
for (j = B - 1; j > 0; --j) {
c[j] = 0.5*(c[j - 1] + c[j]);
}
c[0] = 0.5*(mn + c[0]);
}
void FrameBuffer::Convolve33(M33 kernel, FrameBuffer *&fb1) {
fb1 = new FrameBuffer(30 + w, 30, w, h);
for (int v = 1; v < h-1; v++) {
for (int u = 1; u < w-1; u++) {
V3 newColor(0.0f, 0.0f, 0.0f);
for (int vi = -1; vi <= 1; vi++) {
for (int ui = -1; ui <= 1; ui++) {
V3 currColor;
currColor.setFromColor(Get(u+ui, v+vi));
newColor = newColor + currColor * kernel[vi+1][ui+1];
}
}
unsigned int newc = newColor.getColor();
fb1->Set(u, v, newc);
}
}
}
/**
* Setup this camera according to the specified center, vector a, and the view direction.
*
* @param C the center of the camera
* @param a the vector a (The horizontal direction of the screen)
* @param vd the view direction of the camera
*/
void PPC::Set(const V3& C, const V3& a, const V3& vd) {
this->C = C;
this->a = a;
b = (vd ^ a).UnitVector() * b.Length();
c = vd.UnitVector() * GetFocalLength() - a * ((float)w/2.0f) - b * ((float)h/2.0f);
SetPMat();
}
//d. Draw 2-D segment with color interpolation
void FrameBuffer::Draw2DSegment(V3 p0, V3 c0, V3 p1, V3 c1)
{
float dx = fabsf(p0[0]-p1[0]);
float dy = fabsf(p0[1]-p1[1]);
int n;
if (dx < dy)
n = 1+(int)dy;
else
n = 1+(int)dx;
for (int i = 0; i < n; i++) {
float frac = (float) i / (float) (n-1);
V3 curr = p0 + (p1-p0)*frac;
V3 currc = c0 + (c1-c0)*frac;
int u = (int) curr[0];
int v = (int) curr[1];
SetGuarded(u, v, currc.GetColor());
}
}
