/**************************************************************************
* atct_init:
* calculates alpha1, alpha2, and alpha3, which are some sort of coordinate
* rotation amounts, in degrees. This creates a latitude/longitude-style
* coordinate system centered under the satellite at the start of imaging.
* You must pass it a state vector from the start of imaging. */
void atct_init(meta_projection *proj,stateVector st)
{
vector up={0.0,0.0,1.0};
vector z_orbit, y_axis, a, nd;
double alpha3_sign;
double alpha1,alpha2,alpha3;
vecCross(st.pos,st.vel,&z_orbit);vecNormalize(&z_orbit);
vecCross(z_orbit,up,&y_axis);vecNormalize(&y_axis);
vecCross(y_axis,z_orbit,&a);vecNormalize(&a);
alpha1 = atan2_check(a.y,a.x)*R2D;
alpha2 = -1.0 * asind(a.z);
if (z_orbit.z < 0.0)
{
alpha1 += 180.0;
alpha2 = -1.0*(180.0-fabs(alpha2));
}
vecCross(a,st.pos,&nd);vecNormalize(&nd);
alpha3_sign = vecDot(nd,z_orbit);
alpha3 = acosd(vecDot(a,st.pos)/vecMagnitude(st.pos));
if (alpha3_sign<0.0)
alpha3 *= -1.0;
proj->param.atct.alpha1=alpha1;
proj->param.atct.alpha2=alpha2;
proj->param.atct.alpha3=alpha3;
}
Vec3f GetIntersectionPoint(const Planef &p1, const Planef &p2) {
const Vec3f p = d * vecCross(p1.normal, p2.normal)
+ p1.d * vecCross(p2.normal, normal)
+ p2.d * vecCross(normal, p1.normal);
const float d = -1/vecDot(normal, vecCross(p1.normal, p2.normal));
return p * d;
}
void Frustum::Extract(const Matrix44f &view, const Projection &proj) {
static const int LBN = kLBNCorner, LTN = kLTNCorner, LBF = kLBFCorner,
LTF = kLTFCorner, RBN = kRBNCorner, RTN = kRTNCorner,
RBF = kRBFCorner, RTF = kRTFCorner;
const float lnear = -proj.get_near();
const float lfar = -proj.get_far();
const Vec3f nearNorm(0.0f, 0.0f, -1.0f);
const Vec3f farNorm(0.0f, 0.0f, 1.0f);
// kLeftPlane plane
const Vec3f ltn(proj.get_left(), proj.get_top(), lnear),
lbn(proj.get_left(), proj.get_bottom(), lnear),
ltf(proj.get_far_left(), proj.get_far_top(), lfar),
lbf(proj.get_far_left(), proj.get_far_bottom(), lfar);
const Vec3f leftNorm = vecNormal(
vecCross(lbn - ltn, ltf - ltn));
// kRightPlane plane
const Vec3f rtn(proj.get_right(), proj.get_top(), lnear), // right top near
rbn(proj.get_right(), proj.get_bottom(), lnear), // right bottom near
rtf(proj.get_far_right(), proj.get_far_top(), lfar); // right top far
const Vec3f rightNorm = vecNormal(vecCross(rtf - rtn, rbn - rtn));
// kTopPlane plane
const Vec3f topNorm = vecNormal(vecCross(ltf - ltn, rtn - ltn));
// kBottomPlane plane lbn rbf
const Vec3f rbf(proj.get_far_right(), proj.get_far_bottom(), lfar);
const Vec3f botNorm = vecNormal(vecCross(rbf - rbn, lbn - rbn));
Matrix44f viewSpace, viewSpaceN;
matrixInverse(view, &viewSpace);
viewSpaceN = matrixTranspose(view);
m_conners[LBN] = lbn * viewSpace;
m_conners[LTN] = ltn * viewSpace;
m_conners[LBF] = lbf * viewSpace;
m_conners[LTF] = ltf * viewSpace;
m_conners[RBN] = rbn * viewSpace;
m_conners[RTN] = rtn * viewSpace;
m_conners[RBF] = rbf * viewSpace;
m_conners[RTF] = rtf * viewSpace;
m_planes[kLeftPlane] = Planef(leftNorm * viewSpaceN, m_conners[LTN]);
m_planes[kRightPlane] = Planef(rightNorm * viewSpaceN, m_conners[RTN]);
m_planes[kTopPlane] = Planef(topNorm * viewSpaceN, m_conners[RTN]);
m_planes[kBottomPlane] = Planef(botNorm * viewSpaceN, m_conners[LBN]);
m_planes[kNearPlane] = Planef(nearNorm * viewSpaceN, m_conners[LBN]);
m_planes[kFarPlane] = Planef(farNorm * viewSpaceN, m_conners[RTF]);
}
//------------------------------------------------------------------------------
void Missile::frameMove(float dt)
{
Projectile::frameMove(dt);
updateTarget(dt);
Vector cur_dir = getGlobalLinearVel();
cur_dir.normalize();
Vector target_dir = target_ - getPosition();
float l = target_dir.length();
if (equalsZero(l)) return;
target_dir /= l;
float alpha = acosf(vecDot(&cur_dir, &target_dir));
if (alpha > agility_*dt )
{
Matrix m;
Vector axis;
vecCross(&axis, &target_dir, &cur_dir);
if (!equalsZero(axis.length()))
{
m.loadRotationVector(agility_*dt, axis);
target_dir = m.transformVector(cur_dir);
} else target_dir = cur_dir;
}
setGlobalLinearVel(target_dir*speed_);
}
/*
Get_sep:
Calculates the separation between two satellites along
the satellite beam, in the normal- and parallel- to look direction.
Inputs are: a state vector from scene 1, in GEI coordinates;
the name of the ceos for image 2; the slant range and doppler of
the center of the beam; and pointers to the output normal and
parallel baselines.
*/
void get_sep(stateVector stVec1, meta_parameters *meta2,
double range, double dop,double *Bn,double *Bp)
{
double lat,phi,earthRadius;
vector target,up,relPos;
vector upBeam,alongBeam,beamNormal;
double t,timeDelta;
stateVector stVec2;
GEOLOCATE_REC *g;
/*Target is the patch of ground at beam center.*/
/*Initialize the transformation.*/
g=init_geolocate_meta(&stVec1,meta2);
getLoc(g,range,dop,
&lat,&phi,&earthRadius);
free_geolocate(g);
sph2cart(earthRadius,lat,phi,&target);
/*Create beam plane unit vectors.*/
vecSub(stVec1.pos,target,&alongBeam); vecNormalize(&alongBeam);
up=stVec1.pos; vecNormalize(&up);
vecCross(up,alongBeam,&beamNormal); vecNormalize(&beamNormal);
vecCross(alongBeam,beamNormal,&upBeam); vecNormalize(&upBeam);
/*Find the time when the second satellite crosses the first's beam.*/
t=0.0;
stVec2=meta_get_stVec(meta2,t);
timeDelta=1.0;
while (fabs(timeDelta)>0.00001)
{
vecSub(stVec1.pos,stVec2.pos,&relPos);
timeDelta=vecDot(beamNormal,relPos)/vecDot(beamNormal,stVec2.vel);
t+=timeDelta;
if (!quietflag) {
printf(" Time=%f sec, delta=%f sec\n",t,timeDelta);
printf(" Distance from beam plane=%f m\n",vecDot(beamNormal,relPos));
}
stVec2=meta_get_stVec(meta2,t);
}
/*Now we have the second satellite sitting in the plane of the first's beam,
so we just separate that position into components along-beam and across-beam,
and return.*/
vecSub(stVec2.pos,stVec1.pos,&relPos);
*Bp=vecDot(alongBeam,relPos);
*Bn=vecDot(upBeam,relPos);
}
void matLookAt(Matrix4x4 *m, vec3 camera, vec3 point, vec3 vecNorm){
vec3 forward, side, up;
Matrix4x4 mat, mat2;
memcpy(&mat2, m, sizeof(Matrix4x4));
forward.x = point.x - camera.x;
forward.y = point.y - camera.y;
forward.z = point.z - camera.z;
forward = vecNormalize(forward);
up = vecNorm;
side = vecCross(up, forward);
up = vecCross(forward, side);
side = vecNormalize(side);
up = vecNormalize(up);
// stolen from gluLookat.c
#define M(row,col) mat.m[col*4+row]
M(0,0) = side.x;
M(0,1) = side.y;
M(0,2) = side.z;
M(0,3) = 0.f;
M(1,0) = up.x;
M(1,1) = up.y;
M(1,2) = up.z;
M(1,3) = 0.f;
M(2,0) = -forward.x;
M(2,1) = -forward.y;
M(2,2) = -forward.z;
M(2,3) = 0.f;
M(3,0) = M(3,1) = M(3,2) = 0.f;
M(3,3) = 1.f;
#undef M
// -----------------------
matMul(m, &mat, &mat2);
vecMatTranslate(m, camera);
}
//------------------------------------------------------------------------------
void Plane::create(const Vector & p0, const Vector & p1, const Vector & p2)
{
Vector p0p1 = p1 - p0;
Vector p0p2 = p2 - p0;
vecCross(&normal_, &p0p1, & p0p2);
normal_.normalize();
setPointOnPlane(p0);
}
void triangleNormal(double a[], double b[], double c[], double normal[])
{
int i;
double b_a[3],c_a[3];
for(i = 0; i < 3; i++){
b_a[i] = b[i] - a[i];
c_a[i] = c[i] - a[i];
}
vecCross(b_a,c_a,normal);
vecUnitization(normal,normal);
}
static void
get_target_position(stateVector *st, double look, double yaw,
double sr, vector *targPos)
{
vector relPos = vecNew(sin(yaw), -sin(look)*cos(yaw), -cos(look)*cos(yaw));
// relPos unit vector points from s/c to targPos.
// Rotate into earth axes
vector look_matrix[3];
look_matrix[2] = st->pos;
vecNormalize(&look_matrix[2]);
vector v = st->vel;
vecNormalize(&v);
vecCross(look_matrix[2],v,&look_matrix[1]);
vecCross(look_matrix[1],look_matrix[2],&look_matrix[0]);
vecMul(look_matrix,relPos,&relPos);
// scale relPos so it reaches from s/c to targPos
vecScale(&relPos,sr);
vecAdd(relPos,st->pos,targPos);
}
void updateAppendage(Scene* scene, int sceneIndex, cl_float3 p, cl_float3 q,
cl_float3 w, float A, float B, int countA, int countB) {
cl_float3 U;
cl_float3 V;
cl_float3 W;
cl_float3 j;
cl_float3 d;
V = q;
vecSub(V, p);
float D = vecLength(V);
float inverseD = 1.0f / D;
vecScale(V, inverseD);
W = w;
vecNormalize(W);
vecCross(U, V, W);
float A2 = A*A;
float y = 0.5f * inverseD * (A2 - B * B + D * D);
double square = A2 - y*y;
if (square < 0.0f)
throw std::runtime_error("Unable to construct appendage");
float x = sqrtf(square);
j = p;
vecScaledAdd(j, x, U);
vecScaledAdd(j, y, V);
d = j;
vecSub(d, p);
vecScale(d, 1.0f / float(countA));
for (int i = 0; i <= countA; i++) {
Sphere* sphere = &(scene->spheres[sceneIndex + i]);
sphere->center = p;
vecScaledAdd(sphere->center, float(i), d);
vecScale(sphere->center, 0.1f);
}
d = j;
vecSub(d, q);
vecScale(d, 1.0f / float(countB));
for (int i = 0; i <= countA; i++) {
Sphere* sphere = &(scene->spheres[sceneIndex + i + countA + 1]);
sphere->center = q;
vecScaledAdd(sphere->center, float(i), d);
vecScale(sphere->center, 0.1f);
}
}
void calibrate()
{
// TODO: wait for things to stabilise
//printf("Calibrating\n");
Compass_ReadAccAvg(Zaxis, 1000);
vecNorm(Zaxis);
//printf("Z: %9.3f %9.3f %9.3f\n", Zaxis[0], Zaxis[1], Zaxis[2]);
Compass_ReadMag(Xaxis);
vecNorm(Xaxis);
//printf("X: %9.3f %9.3f %9.3f\n", Xaxis[0], Xaxis[1], Xaxis[2]);
vecCross(Yaxis, Zaxis, Xaxis);
vecNorm(Yaxis);
vecCross(Xaxis, Yaxis, Zaxis);
vecNorm(Xaxis);
Gyro_ReadAngRateAvg(zeroAngRate, 100);
//printf("X: %9.3f %9.3f %9.3f\n", Xaxis[0], Xaxis[1], Xaxis[2]);
//printf("Y: %9.3f %9.3f %9.3f\n", Yaxis[0], Yaxis[1], Yaxis[2]);
//printf("Z: %9.3f %9.3f %9.3f\n", Zaxis[0], Zaxis[1], Zaxis[2]);
}
void calibrate()
{
// at rest the accelerometer reports an acceleration straight upwards
// due to gravity. use this as our Z axis
Compass_ReadAccAvg(Zaxis, 500);
vecNorm(Zaxis);
// the magnetic strength is greatest towards magnetic north. use this as
// our X axis
Compass_ReadMagAvg(Xaxis, 10);
vecNorm(Xaxis);
// create a Y axis perpendicular to the X and Z axes
vecCross(Yaxis, Zaxis, Xaxis);
vecNorm(Yaxis);
// ensire that the X axis is actually perpendicular to the Y and Z axes
vecCross(Xaxis, Yaxis, Zaxis);
vecNorm(Xaxis);
// calibrate the zero error on the gyroscope
Gyro_ReadAngRateAvg(zeroAngRate, 100);
}
void Line::computePrism()
{
// These vectors span the rectangular integration prism
p_c = effectivePosB() - effectivePosA();
p_a[0] = p_c[2];
p_a[1] = 0;
p_a[2] = -p_c[0];
p_a = vecNormalize(p_a) * p_prism_side_a;
p_b = vecNormalize(vecCross(p_c, p_a)) * p_prism_side_b; // The arguments of the cross product are not permutable
}
void rotateMatrix(double axis[], double mRotate[][4], double mTransform[][4], double radian)
{
int i,j;
double u[3],v[3],t[3];
double tmp[4][4];
vecUnitization(axis,axis);
t[0] = axis[0];
t[1] = axis[1]+1;
t[2] = axis[2];
vecCross(t,axis,u);
vecUnitization(u,u);
vecCross(axis,u,v);
vecUnitization(v,v);
matrixInitial(tmp);
matrixInitial(mRotate);
tmp[0][0] = cos(radian);
tmp[0][1] = -sin(radian);
tmp[1][0] = sin(radian);
tmp[1][1] = cos(radian);
for(i = 0; i < 3; i++){
mRotate[0][i] = u[i];
mRotate[1][i] = v[i];
mRotate[2][i] = axis[i];
}
matrixMultiply(tmp,mRotate,0);
matrixInitial(tmp);
for(i = 0; i < 3; i++){
tmp[i][0] = u[i];
tmp[i][1] = v[i];
tmp[i][2] = axis[i];
}
matrixMultiply(tmp,mRotate,0);
matrixMultiply(mRotate,mTransform,0);
}
// gluLookAt() equivalent
void matrixLookAt( float result[16],
const float eye[3],
const float target[3],
const float up[3])
{
float forward[3] = { target[0] - eye[0],
target[1] - eye[1],
target[2] - eye[2]
};
vecNormalize(forward);
float side[3];
vecCross(side, forward, up);
float up_after[3];
vecCross(up_after, side, forward);
float view_matrix[16] = MATRIX_IDENTITY;
view_matrix[0] = side[0];
view_matrix[4] = side[1];
view_matrix[8] = side[2];
view_matrix[1] = up_after[0];
view_matrix[5] = up_after[1];
view_matrix[9] = up_after[2];
view_matrix[2] = -forward[0];
view_matrix[6] = -forward[1];
view_matrix[10] = -forward[2];
// Final translation
float trans_matrix[16];
float minus_eye[3] = {-eye[0], -eye[1], -eye[2]};
matrixTranslate(trans_matrix, minus_eye);
matrixMult(result, view_matrix, trans_matrix);
}
/**
* Updates wheel rotation and adds contact joints for tire ray
* contact points.
*/
void Tank::frameMoveTires(float dt)
{
ADD_STATIC_CONSOLE_VAR(bool, calc_tire_physics, true);
if (!calc_tire_physics) return;
// Handle steering angle
// Return to center position very quickly
if (sign(target_steer_angle_) != input_.left_ - input_.right_)
{
target_steer_angle_ *= steer_retreat_factor_;
}
if (input_.left_ - input_.right_)
{
target_steer_angle_ += dt * steer_speed_ * (input_.left_ - input_.right_);
if (abs(target_steer_angle_) > max_steer_angle_)
{
target_steer_angle_ = sign(target_steer_angle_) * max_steer_angle_;
}
}
float dir_vel = -target_object_->getLocalLinearVel().z_;
is_braking_ = ((input_.up_ - input_.down_) > 0 && dir_vel < 0) ||
((input_.up_ - input_.down_) < 0 && dir_vel > 0);
Matrix tank_transform = getTransform();
Vector dir = -tank_transform.getZ();
// Calc erp and cfm from suspension spring parameters (see ODE docs)
float cfm = 1.0f / (dt* suspension_k_ + suspension_d_);
float erp = dt* suspension_k_*cfm;
dContact contact;
memset(&contact, 0, sizeof(contact));
contact.surface.mode =
dContactSoftERP | dContactSoftCFM |
dContactFDir1 | dContactApprox1 | dContactSlip2 |
dContactMu2;
contact.surface.soft_erp = erp;
contact.surface.soft_cfm = cfm;
// Force-dependent-slip is proportional to long. moving speed
contact.surface.slip2 = abs(dir_vel) * force_dependent_slip_;
for (unsigned w=0; w<wheel_.size(); ++w)
{
bool braking = is_braking_ || (wheel_[w].handbraked_ && input_.action2_);
doTerrainWheelCollision(wheel_[w], tank_transform, true);
if (wheel_[w].collision_info_.penetration_ != 0.0f)
{
contact.surface.mu2 = (braking ? static_mu_lat_brake_ : static_mu_lat_) * inv_gravity_;
Vector fdir = dir - wheel_[w].collision_info_.n_ * vecDot(&dir, &wheel_[w].collision_info_.n_);
fdir.normalize();
Vector right;
vecCross(&right, &wheel_[w].collision_info_.n_, &fdir);
float wheel_steer_angle = target_steer_angle_ * wheel_[w].steer_factor_;
fdir = cosf(wheel_steer_angle) * fdir + sinf(wheel_steer_angle) * right;
contact.fdir1[0] = fdir.x_;
contact.fdir1[1] = fdir.y_;
contact.fdir1[2] = fdir.z_;
if (braking)
{
contact.surface.mode &= ~dContactMotion1;
contact.surface.mu = static_mu_long_brake_ * inv_gravity_;
} else if (wheel_[w].driven_)
{
if (input_.up_ != input_.down_)
{
contact.surface.mode |= dContactMotion1;
contact.surface.motion1 = (input_.up_ - input_.down_) * max_speed_;
contact.surface.mu = static_mu_long_acc_ * inv_gravity_;
} else
{
contact.surface.mode &= ~dContactMotion1;
contact.surface.mu = engine_brake_mu_ * inv_gravity_;
}
} else
{
// Cannot set zero here or fdir is ignored
contact.surface.mode &= ~dContactMotion1;
contact.surface.mu = rolling_mu_ * inv_gravity_;
}
contact.geom.pos[0] = wheel_[w].collision_info_.pos_.x_;
contact.geom.pos[1] = wheel_[w].collision_info_.pos_.y_;
contact.geom.pos[2] = wheel_[w].collision_info_.pos_.z_;
contact.geom.normal[0] = wheel_[w].collision_info_.n_.x_;
contact.geom.normal[1] = wheel_[w].collision_info_.n_.y_;
contact.geom.normal[2] = wheel_[w].collision_info_.n_.z_;
contact.geom.depth = wheel_[w].collision_info_.penetration_;
contact.geom.g1 = wheel_[w].collision_info_.this_geom_->getId();
target_object_->getSimulator()->addContactJoint(contact, target_object_, NULL);
}
// Reset penetration for next frame.
wheel_[w].collision_info_.penetration_ = 0.0f;
}
}
/**
* Creates a plane from a triangle, the plane normal is not normalized.
* Assumes that the points are in counter clock wise order (OpenGL).
* @param a triangle point
* @param b triangle point
* @param c triangle point
*/
Planef(const Vec3f &a, const Vec3f &b, const Vec3f &c) {
normal = vecCross(b - a, c - b);
d = -vecDot(normal, a);
}
void GLRender::moveCamera(int value)
{
startStoreViewImageAndPose = 1;
// define camera coordinate system
GLRenderVec x_axis, y_axis, z_axis, up;
x_axis.x = 1; x_axis.y = 0; x_axis.z = 0;
y_axis.x = 0; y_axis.y = 1; y_axis.z = 0;
z_axis.x = 0; z_axis.y = 0; z_axis.z = 1;
up.x = 0; up.y = 0; up.z = 1;
cv::Matx33f cs = cv::Matx33f::eye();
// change scale
if(m_radius<=m_maxRadius)
{
// change latitude
if(m_latitude<=m_maxLatitude)
{
float lati_radian = m_latitude*PI/180;
// when at north polar or south polar, ignore the longitude change
if(m_latitude==90.0f || m_latitude==-90.0f)
{
// camera origin location
m_translation[0] = 0.0f;
m_translation[1] = 0.0f;
m_translation[2] = m_radius;
// change inplane rotation
if(m_inplane<=m_maxInplane)
{
// view direction axis
float axis[3] = {-m_translation[0]/m_radius, -m_translation[1]/m_radius, -m_translation[2]/m_radius};
// calculate camera coordinate system axises
z_axis.x = -axis[0]; z_axis.y = -axis[1]; z_axis.z = -axis[2];
x_axis.x = 1; x_axis.y = 0; x_axis.z = 0;
y_axis.x = 0; y_axis.y = 1; y_axis.z = 0;
cs(0,0) = x_axis.x; cs(0,1) = y_axis.x; cs(0,2) = z_axis.x;
cs(1,0) = x_axis.y; cs(1,1) = y_axis.y; cs(1,2) = z_axis.y;
cs(2,0) = x_axis.z; cs(2,1) = y_axis.z; cs(2,2) = z_axis.z;
// define inplate rotation matrix
// rotation alone the z-axis (i.e. the inversed view direction)
float inplane_radian = m_inplane*PI/180;
cv::Matx33f inplaneRot = cv::Matx33f::eye();
inplaneRot(0,0) = cos(inplane_radian); inplaneRot(0,1) = -sin(inplane_radian);
inplaneRot(1,0) = sin(inplane_radian); inplaneRot(1,1) = cos(inplane_radian);
cs = cs*inplaneRot;
// represent the camera coordinate system using Rodrigues formate
cv::Matx31f cs_rodrigues;
cv::Rodrigues(cs, cs_rodrigues);
m_rotation[0] = cs_rodrigues(0);
m_rotation[1] = cs_rodrigues(1);
m_rotation[2] = cs_rodrigues(2);
// go to next inplane rotation state
m_inplane+=m_inplaneStep;
++viewCounter;
// animation
glutPostRedisplay();
glutTimerFunc(10, moveCamera, 1);
return;
}
}
else
{
// change longitude
if(m_longitude<=m_maxLongitude)
{
float longi_radian = m_longitude*PI/180;
// define camera origin location
m_translation[0] = m_radius * cos(lati_radian) * cos(longi_radian);
m_translation[1] = m_radius * cos(lati_radian) * sin(longi_radian);
m_translation[2] = m_radius * sin(lati_radian);
// change inplane rotation
if(m_inplane<=m_maxInplane)
{
// view direction axis
float axis[3] = {-m_translation[0]/m_radius, -m_translation[1]/m_radius, -m_translation[2]/m_radius};
// calculate camera coordinate system axises
z_axis.x = -axis[0]; z_axis.y = -axis[1]; z_axis.z = -axis[2];
x_axis = vecCross(up, z_axis);
float x_axis_norm = vecNorm(x_axis);
if(x_axis_norm>0)
x_axis.x /= x_axis_norm; x_axis.y /= x_axis_norm; x_axis.z /= x_axis_norm;
y_axis = vecCross(z_axis, x_axis);
float y_axis_norm = vecNorm(y_axis);
if(y_axis_norm>0)
y_axis.x /= y_axis_norm; y_axis.y /= y_axis_norm; y_axis.z /= y_axis_norm;
cs(0,0) = x_axis.x; cs(0,1) = y_axis.x; cs(0,2) = z_axis.x;
cs(1,0) = x_axis.y; cs(1,1) = y_axis.y; cs(1,2) = z_axis.y;
cs(2,0) = x_axis.z; cs(2,1) = y_axis.z; cs(2,2) = z_axis.z;
// define inplate rotation matrix
// rotation alone the z-axis (i.e. the inversed view direction)
float inplane_radian = m_inplane*PI/180;
cv::Matx33f inplaneRot = cv::Matx33f::eye();
inplaneRot(0,0) = cos(inplane_radian); inplaneRot(0,1) = -sin(inplane_radian);
inplaneRot(1,0) = sin(inplane_radian); inplaneRot(1,1) = cos(inplane_radian);
cs = cs*inplaneRot;
// represent the camera coordinate system using Rodrigues formate
cv::Matx31f cs_rodrigues;
cv::Rodrigues(cs, cs_rodrigues);
m_rotation[0] = cs_rodrigues(0);
m_rotation[1] = cs_rodrigues(1);
m_rotation[2] = cs_rodrigues(2);
// go to next inplane rotation state
m_inplane+=m_inplaneStep;
++viewCounter;
// animation
glutPostRedisplay();
glutTimerFunc(10, moveCamera, 1);
return;
}
m_longitude+=m_longitudeStep;
m_inplane = m_minInplane;
// animation
glutTimerFunc(10, moveCamera, 1);
return;
}
}
m_latitude+=m_latitudeStep;
m_longitude = m_minLongitude;
m_inplane = m_minInplane;
// animation
glutTimerFunc(10, moveCamera, 1);
return;
}
m_radius+=m_radiusStep;
m_latitude = m_minLatitude;
m_longitude = m_minLongitude;
m_inplane = m_minInplane;
// animation
glutTimerFunc(10, moveCamera, 1);
return;
}
// done the camera viewpoints sampling and exit the main loop
// close the opengl context
poseOutFile.close();
glutLeaveMainLoop();
}
void triangleVertexShading(JmJob *data)
{
VertexJob v;
dmaBlockGet(&v, (uintptr_t)data->p1, sizeof(VertexJob), DmaTag);
Vec halfSizeAdd = vec(0.5f * sw, 0.5f * sh, 0, 1);
Vec halfSizeMul = vec(0.5f * sw, -0.5f * sh, 1, 0);
Vec zero = vec(0);
Mat transform;
matMul(&transform, v.projection, v.world);
while(true)
{
unsigned int k = interlockedExchangeAdd(v.counter, VerticesAtATime);
if(k >= v.end) break;
unsigned int end = std::min(k + VerticesAtATime, v.end);
unsigned int num = end - k;
unsigned int numtris = num / 3;
#ifndef PLATFORM_PS3_SPU
Triangle3D vertexShadingTris[TriangleAtATime];
#endif
dmaBlockGet(vertexShadingTris, (uintptr_t)&v.input[k], numtris * sizeof(Triangle3D), DmaTag);
for(unsigned int u = 0; u < numtris; u++)
{
const Triangle3D &tri = vertexShadingTris[u];
Triangle3D triOut[5];
Vec vtx[8];
int numTrisOut = 1;
if(g_EnableBackfaceCulling)
{
// We can't do the backface culling using a determinant of a 2x2 matrix
// in screen space because then we would have 'holes' in the output data
// therefor it happens here, in wordspace.
Vec e1 = vecSub(tri.p3, tri.p1);
Vec e2 = vecSub(tri.p2, tri.p1);
Vec n = vecCross(e1, e2);
Vec a = vecDot(v.camerapos, n);
if(vecGetElem(a, VecComponent::X) > 0) continue;
}
// perspective project
matMulVec(&triOut[0].p1, transform, tri.p1);
matMulVec(&triOut[0].p2, transform, tri.p2);
matMulVec(&triOut[0].p3, transform, tri.p3);
// cull against znear
Vec m1 = vecCmpLE(vecSplat<VecComponent::Z>(triOut[0].p1), zero);
Vec m2 = vecCmpLE(vecSplat<VecComponent::Z>(triOut[0].p2), zero);
Vec m3 = vecCmpLE(vecSplat<VecComponent::Z>(triOut[0].p3), zero);
Vec c2 = vecAnd(vecAnd(m1, m2), m3);
#ifdef PLATFORM_PS3
vec_uint4 ones = (vec_uint4){0xffffffff,0xffffffff,0xffffffff,0xffffffff};
if(vec_all_eq((vec_uint4)c2, ones)) continue;
#else
int result = vecMaskToInt(c2);
if(result == 15) continue; // discard, all behind nearz
#endif
#if 1
// clip triangles that intersect znear
static const int NumVerticesInATriangle = 3;
int numVertsOut = triangleClipToPlane(
(Vec*)&triOut[0],
vtx,
NumVerticesInATriangle,
vec(0, 0, 1, 0)
);
// Very simple triangulation routine
numTrisOut = 0;
for(int i = 2; i < numVertsOut; i++)
{
triOut[numTrisOut].p1 = vtx[0];
triOut[numTrisOut].p2 = vtx[i];
triOut[numTrisOut].p3 = vtx[i - 1];
numTrisOut++;
}
#endif
for(int i = 0; i < numTrisOut; i++)
{
// perspective divide
triOut[i].p1 = vecMul(triOut[i].p1, vecRcp(vecSplat<VecComponent::W>(triOut[i].p1)));
triOut[i].p2 = vecMul(triOut[i].p2, vecRcp(vecSplat<VecComponent::W>(triOut[i].p2)));
triOut[i].p3 = vecMul(triOut[i].p3, vecRcp(vecSplat<VecComponent::W>(triOut[i].p3)));
// transform to screen space
Vec r1 = vecMadd(triOut[i].p1, halfSizeMul, halfSizeAdd);
Vec r2 = vecMadd(triOut[i].p2, halfSizeMul, halfSizeAdd);
Vec r3 = vecMadd(triOut[i].p3, halfSizeMul, halfSizeAdd);
#ifdef PLATFORM_PS3_SPU
Triangle3DSetup &r = setup[s][sidx];
#else
Triangle3DSetup r;
#endif
memcpy(&r.x1, &r1, sizeof(float) * 3);
memcpy(&r.x2, &r2, sizeof(float) * 3);
memcpy(&r.x3, &r3, sizeof(float) * 3);
// deltas
r.dx1 = r.x1 - r.x2;
r.dx2 = r.x2 - r.x3;
r.dx3 = r.x3 - r.x1;
r.dy1 = r.y1 - r.y2;
r.dy2 = r.y2 - r.y3;
r.dy3 = r.y3 - r.y1;
#ifdef PLATFORM_PS3_SPU
sidx++;
if(sidx >= MaxSetupBuffered)
{
dmaWaitAll(1 << DmaListTag);
unsigned int l = interlockedExchangeAdd(v.outputCnt, sidx);
for(unsigned int u = 0; u < sidx; u++)
{
setuplist[u].notify = 0;
setuplist[u].reserved = 0;
setuplist[u].size = sizeof(Triangle3DSetup);
setuplist[u].eal = (uintptr_t)&v.output[u + l];
}
cellDmaListPut(setup[s], 0, setuplist, sizeof(setuplist), DmaListTag, 0, 0);
sidx = 0;
s ^= 1;
}
#else
unsigned int l = interlockedExchangeAdd(v.outputCnt, 1);
if(l >= MaxTrianglesDrawn) { interlockedExchangeSub(v.outputCnt, 1); break; }
else dmaBlockPut(&r, (uintptr_t)&v.output[l], sizeof(Triangle3DSetup), DmaTag);
#endif
}
}
}
#ifdef PLATFORM_PS3_SPU
if(sidx > 0)
{
dmaWaitAll(1 << DmaListTag);
unsigned int l = interlockedExchangeAdd(v.outputCnt, sidx);
for(unsigned int u = 0; u < sidx; u++)
{
setuplist[u].notify = 0;
setuplist[u].reserved = 0;
setuplist[u].size = sizeof(Triangle3DSetup);
setuplist[u].eal = (uintptr_t)&v.output[u + l];
}
cellDmaListPut(setup[s], 0, setuplist, sidx * sizeof(CellDmaListElement), DmaListTag + 1, 0, 0);
dmaWaitAll(1 << (DmaListTag + 1));
}
#endif
}
int Render_SSD(SCENE *ascene, CAMERA *acamera)
{
/* We clear all pixels */
glClearColor(ascene->bcolor.rgba[0], ascene->bcolor.rgba[1],
ascene->bcolor.rgba[2], ascene->bcolor.rgba[3]);
glClear (GL_COLOR_BUFFER_BIT);
int i,j;
double matrixFinal[4][4],mTransform[4][4];
double intermediaM[4][4],mCam[4][4],mInverse[4][4],tmpMatrix[4][4];
double mTranslate[4][4],mRotate[4][4],mScale[4][4];
double homo_coordinates[4] = {0,0,0,1};
//double mInverse[4][4];
matrixInitial(matrixFinal);
matrixInitial(mTransform);
matrixInitial(mTranslate);
matrixInitial(mRotate);
matrixInitial(mScale);
int screenW = ascene->screen_w;
int screenH = ascene->screen_h;
buffer = (HIDDEN *)malloc(sizeof(HIDDEN) * screenW * screenH);
for(i = 0;i < screenW * screenH;i++)
{
buffer[i].rgba[0] = ascene->bcolor.rgba[0];
buffer[i].rgba[1] = ascene->bcolor.rgba[1];
buffer[i].rgba[2] = ascene->bcolor.rgba[2];
buffer[i].z = 9999;
}
/* Camera View */
int ii;
matrixInitial(mInverse);
matrixInitial(tmpMatrix);
matrixInitial(intermediaM);
matrixInitial(mCam);
double u[3],v[3],w[3];
double gaze[3] = {acamera->gaze.xyzw[0],acamera->gaze.xyzw[1],acamera->gaze.xyzw[2]};
double upVector[3] = {acamera->upVector.xyzw[0],acamera->upVector.xyzw[1],acamera->upVector.xyzw[2]};
vecUnitization(gaze,w);
for(ii = 0; ii < 3; ii++){
w[ii] = -w[ii];
}
vecCross(upVector,w,u);
vecUnitization(u,u);
vecCross(w,u,v);
ii = 0;
for(ii = 0; ii < 3; ii++){
intermediaM[0][ii] = u[ii];
intermediaM[1][ii] = v[ii];
intermediaM[2][ii] = w[ii];
mCam[ii][3] = -acamera->eye.xyzw[ii];
}
matrixMultiply(intermediaM,mCam,0);
/*inverse matrixMultiply() 0:normal 1:inverse */
matrixInitial(intermediaM);
ii = 0;
for(ii = 0; ii < 3; ii++){
intermediaM[ii][0] = u[ii];
intermediaM[ii][1] = v[ii];
intermediaM[ii][2] = w[ii];
tmpMatrix[ii][3] = acamera->eye.xyzw[ii];
}
matrixMultiply(mInverse,tmpMatrix,1);
matrixMultiply(mInverse,intermediaM,1);
/*Persective(1) and Orthographic(0) Projection matrix*/
double anglePers, nearPers, farPers, rightOrtho, topOrtho, farOrtho, nearOrtho,pjType;
double screenWidth,screenHeight;
screenWidth = ascene->screen_w;
screenHeight = ascene->screen_h;
anglePers = ascene->persp.angle;
nearPers = ascene->persp.near;
farPers = ascene->persp.far;
rightOrtho = ascene->ortho.right;
topOrtho = ascene->ortho.top;
farOrtho = ascene->ortho.far;
nearOrtho = ascene->ortho.near;
pjType = ascene->pjType;
getFinalTransformMatrix(anglePers, nearPers, farPers, rightOrtho, topOrtho, farOrtho, nearOrtho, pjType, screenWidth, screenHeight, mCam, matrixFinal,mInverse);
double vpInverse[4][4];
matrixInitial(vpInverse);
vpInverse[0][0] = (double)2/ascene->screen_w;
vpInverse[0][3] = (double)(1-ascene->screen_w)/ascene->screen_w;
vpInverse[1][1] = (double)2/ascene->screen_h;
vpInverse[1][3] = (double)(1-ascene->screen_h)/ascene->screen_h;
matrixMultiply(mInverse,vpInverse,1);
/* Draw floor */
double xmin,xmax,ymin,ymax,floorEdge;
int nX,nY;
xmin = ascene->floor.xmin;
xmax = ascene->floor.xmax;
ymin = ascene->floor.ymin;
ymax = ascene->floor.ymax;
floorEdge = ascene->floor.size;
nY = ((xmax-xmin)/floorEdge) + 1;
nX = ((ymax-ymin)/floorEdge) + 1;
Line floor[nX + nY];
glLineWidth(2);
glBegin(GL_LINES);
glColor3f(ascene->floor.color.rgba[0],ascene->floor.color.rgba[1],ascene->floor.color.rgba[2]);
drawFloor(xmin,xmax,ymin,ymax,nX,nY,floorEdge,matrixFinal,floor);
glEnd();
/* draw axis*/
glLineWidth(ascene->axis.width);
glBegin(GL_LINES);
if(ascene->isAxis == 1){
double origin[4] = {0,0,0,1};
double axisX[4] = {ascene->axis.length,0,0,1};
double axisY[4] = {0,ascene->axis.length,0,1};
double axisZ[4] = {0,0,ascene->axis.length,1};
matrixApply(matrixFinal,origin);
matrixApply(matrixFinal,axisX);
matrixApply(matrixFinal,axisY);
matrixApply(matrixFinal,axisZ);
glColor3f(1,0,0);
glVertex2d(origin[0]/origin[3],origin[1]/origin[3]);
glVertex2d(axisX[0]/axisX[3],axisX[1]/axisX[3]);
glColor3f(0,1,0);
glVertex2d(origin[0]/origin[3],origin[1]/origin[3]);
glVertex2d(axisY[0]/axisY[3],axisY[1]/axisY[3]);
glColor3f(0,0,1);
glVertex2d(origin[0]/origin[3],origin[1]/origin[3]);
glVertex2d(axisZ[0]/axisZ[3],axisZ[1]/axisZ[3]);
}
glEnd();
/* implement objects*/
int nT = 0;
int nR = 0;
int nS = 0;
int nM = 0;
for(i = 0; i < ascene->nidentities; i++){
matrixInitial(mTransform);
for(j = 0; j < ascene->identities[i].inStr_num; j++){
if(ascene->identities[i].instr[j] == TRANSLATE_KEY){
matrixInitial(mTranslate);
mTranslate[0][3] = ascene->translate[nT].xyz[0];
mTranslate[1][3] = ascene->translate[nT].xyz[1];
mTranslate[2][3] = ascene->translate[nT].xyz[2];
matrixMultiply(mTranslate,mTransform,0);
nT++;
}
else if(ascene->identities[i].instr[j] == ROTATE_KEY){
double axis[3];
axis[0] = ascene->rotate[nR].xyz[0];
axis[1] = ascene->rotate[nR].xyz[1];
axis[2] = ascene->rotate[nR].xyz[2];
double Pi = 3.141592653;
double radian = (ascene->rotate[nR].angle/(double)180) * Pi;
rotateMatrix(axis,mRotate,mTransform,radian);
nR++;
}
else if(ascene->identities[i].instr[j] == SCALE_KEY){
matrixInitial(mScale);
mScale[0][0] = ascene->scale[nS].xyz[0];
mScale[1][1] = ascene->scale[nS].xyz[1];
mScale[2][2] = ascene->scale[nS].xyz[2];
matrixMultiply(mScale,mTransform,0);
nS++;
}
else if(ascene->identities[i].instr[j] == MESH_KEY){
double tM[4][4];
matrixInitial(tM);
matrixMultiply(mTransform,tM,0);
matrixMultiply(matrixFinal,tM,0);
int k,l,d;
/*apply transform matrix to all vertices in world coordinates*/
COLOR_VERTEX colorVertices[ascene->mesh[nM].nvertices];
for(k = 0; k < ascene->mesh[nM].nvertices; k++){
colorVertices[k] = ascene->mesh[nM].vertices[k];
matrixApply(mTransform,colorVertices[k].xyzw);
}
//flat shading
if(ascene->mesh[nM].shading == 0){
for(k = 0; k < ascene->mesh[nM].npolygons; k++){
COLOR_VERTEX vertices[3] = {colorVertices[ascene->mesh[nM].polygons[k].num[0]],
colorVertices[ascene->mesh[nM].polygons[k].num[1]],
colorVertices[ascene->mesh[nM].polygons[k].num[2]]};
double normal[3];
triangleNormal(vertices[0].xyzw,vertices[1].xyzw,vertices[2].xyzw,normal);
double center[3];
center[0] = (vertices[0].xyzw[0] + vertices[1].xyzw[0] + vertices[2].xyzw[0])/(double)3;
center[1] = (vertices[0].xyzw[1] + vertices[1].xyzw[1] + vertices[2].xyzw[1])/(double)3;
center[2] = (vertices[0].xyzw[2] + vertices[1].xyzw[2] + vertices[2].xyzw[2])/(double)3;
Illumination(normal,ascene->mesh[nM].diffuse,ascene->mesh[nM].specular,center);
int i;
for(i = 0; i < 3; i++){ matrixApply(matrixFinal,vertices[i].xyzw);
}
toScreen(vertices[0].xyzw,vertices[1].xyzw,vertices[2].xyzw); triRendering(vertices[0].xyzw,vertices[1].xyzw,vertices[2].xyzw,0,0,0,0,0,0,mInverse);
}
}
//phong shading
else if (ascene->mesh[nM].shading == 2){
double vertex_normal[ascene->mesh[nM].nvertices][3];
for(k = 0; k < ascene->mesh[nM].nvertices; k++){
double composeNormal[3] = {0,0,0};
for(l = 0; l < ascene->mesh[nM].npolygons; l++){
for(d = 0; d < ascene->mesh[nM].polygons[l].nvertices; d++){
if(ascene->mesh[nM].polygons[l].num[d] == k){
COLOR_VERTEX vertices[3] = {colorVertices[ascene->mesh[nM].polygons[l].num[0]],
colorVertices[ascene->mesh[nM].polygons[l].num[1]],
colorVertices[ascene->mesh[nM].polygons[l].num[2]]};
double normal[3];
triangleNormal(vertices[0].xyzw,vertices[1].xyzw,vertices[2].xyzw,normal);
composeNormal[0] += normal[0];
composeNormal[1] += normal[1];
composeNormal[2] += normal[2];
}
}
} vecUnitization(composeNormal,composeNormal);
int i=0;
for(i = 0; i < 3; i++){
colorVertices[k].rgba[i] = composeNormal[i];
}
}
setBuffer(colorVertices,matrixFinal,nM,ascene->mesh[nM].diffuse,ascene->mesh[nM].specular,2,mInverse);
}
//smooth shading
else{
for(k = 0; k < ascene->mesh[nM].nvertices; k++){
double composeNormal[3] = {0,0,0};
for(l = 0; l < ascene->mesh[nM].npolygons; l++){
for(d = 0; d < ascene->mesh[nM].polygons[l].nvertices; d++){
if(ascene->mesh[nM].polygons[l].num[d] == k){
COLOR_VERTEX vertices[3] = {colorVertices[ascene->mesh[nM].polygons[l].num[0]],
colorVertices[ascene->mesh[nM].polygons[l].num[1]],
colorVertices[ascene->mesh[nM].polygons[l].num[2]]};
double normal[3];
triangleNormal(vertices[0].xyzw,vertices[1].xyzw,vertices[2].xyzw,normal);
composeNormal[0] += normal[0]; composeNormal[1] += normal[1]; composeNormal[2] += normal[2];
}
}
}
vecUnitization(composeNormal,composeNormal);
Illumination(composeNormal,ascene->mesh[nM].diffuse,ascene->mesh[nM].specular,colorVertices[k].xyzw);
colorVertices[k].rgba[0] = illuColor[0];
colorVertices[k].rgba[1] = illuColor[1];
colorVertices[k].rgba[2] = illuColor[2];
}
setBuffer(colorVertices,matrixFinal,nM,0,0,1,mInverse);
}
glLineWidth(ascene->mesh[nM].width);
glBegin(GL_POINTS);
for(k = 0; k < ascene->screen_h; k++){
for(l = 0; l < ascene->screen_w; l++){
if(buffer[k*(ascene->screen_w)+l].z < 9999){ glColor3f(buffer[k*(ascene->screen_w) + l].rgba[0],buffer[k*(ascene->screen_w) + l].rgba[1],buffer[k*(ascene->screen_w) + l].rgba[2]); glVertex2i(l,k);
}
}
}
glEnd();
nM++;
}
}
}
for(i = 0; i < ascene->nlines; i++){
ascene->lines[i].vertices[0].xyzw[3] = 1;
ascene->lines[i].vertices[1].xyzw[3] = 1;
COLOR_VERTEX vertices[2];
vertices[0] = ascene->lines[i].vertices[0];
vertices[1] = ascene->lines[i].vertices[1];
matrixApply(matrixFinal,vertices[0].xyzw);
matrixApply(matrixFinal,vertices[1].xyzw);
glLineWidth(ascene->lines[i].width);
glBegin(GL_LINES);
glColor3f(vertices[0].rgba[0],vertices[0].rgba[1],vertices[0].rgba[2]);
glVertex2d(vertices[0].xyzw[0]/vertices[0].xyzw[3],vertices[0].xyzw[1]/vertices[0].xyzw[3]);
glColor3f(vertices[1].rgba[0],vertices[1].rgba[1],vertices[1].rgba[2]);
glVertex2d(vertices[1].xyzw[0]/vertices[1].xyzw[3],vertices[1].xyzw[1]/vertices[1].xyzw[3]);
glEnd();
}
free(buffer);
glFlush ();
glutSwapBuffers();
return 0;
}
