void DirectLight::apply()
{
const Matrix4x4& wt = cachedWorldTransform();
Vector3 wrotz = Vector3( wt(0,2),wt(1,2),wt(2,2) );
float wrotzlen = wrotz.length();
if ( wrotzlen > Float::MIN_VALUE )
{
wrotz *= 1.f / wrotzlen;
float intens = intensity();
gd::LightState ls;
GdUtil::setLightState( ls,
gd::LightState::LIGHT_DIRECT,
diffuseColor()*intens, specularColor()*intens, ambientColor()*intens,
wt.translation(), wrotz.normalize(),
MAX_RANGE, 1.f, 0.f, 0.f,
1.f, MAX_CONE_ANGLE, MAX_CONE_ANGLE );
gd::GraphicsDevice* dev = Context::device();
dev->addLight( ls );
}
}
void Rect::draw(){
Vector3 n = u[0] * u[1];
Vector3 ux = r[0]*u[0];
Vector3 uy = r[1]*u[1];
Point3 pt;
n.normalize();
glBindTexture(GL_TEXTURE_2D, texID);
glBegin(GL_POLYGON);
glNormal3d(n.x, n.y, n.z);
pt = c + ux + uy;
glTexCoord2f(1.0, 0.0);
glVertex3d(pt.x, pt.y, pt.z);
pt = c +(-ux) + uy;
glTexCoord2f(1.0, 1.0);
glVertex3d(pt.x, pt.y, pt.z);
pt = c + (-ux) + (-uy);
glTexCoord2f(0.0, 1.0);
glVertex3d(pt.x, pt.y, pt.z);
pt = c + ux + (-uy);
glTexCoord2f(0.0, 0.0);
glVertex3d(pt.x, pt.y, pt.z);
glEnd();
}
void Matrix4::rotate(Vector3& v, float t){
t=BasicMath::radian((double)t);
//normalize v
v.normalize();
float c=cos(t);
float s=sin(t);
m[0][0] = v[0]*v[0]+c*(1- v[0]*v[0]);
m[0][1] = v[0]*v[1]*(1-c)+v[2]*s;
m[0][2] = v[0]*v[2]*(1-c)-v[1]*s;
m[0][3] = 0;
m[1][0] = v[0]*v[1]*(1-c)-v[2]*s;
m[1][1] = v[1]*v[1]+c*(1- v[1]*v[1]);
m[1][2] = v[1]*v[2]*(1-c)+v[0]*s;
m[1][3] = 0;
m[2][0] = v[0]*v[2]*(1-c)+v[1]*s;
m[2][1] = v[2]*v[3]*(1-c)-v[0]*s;
m[2][2] = v[2]*v[2]+c*(1- v[2]*v[2]);
m[2][3] = 0;
m[3][0] = 0;
m[3][1] = 0;
m[3][2] = 0;
m[3][3] = 1;
}
GodRaysResult GodRaysSampler::getResult(const SpaceSegment &segment) {
Vector3 step = segment.getEnd().sub(segment.getStart());
double max_length = step.getNorm();
step = step.normalize().scale(walk_step);
Vector3 walker = segment.getStart();
double travelled = 0.0;
double inside = 0.0;
if (max_length > this->max_length) {
max_length = this->max_length;
}
while (travelled < max_length) {
double light = getCachedLight(walker);
inside += light * walk_step;
walker = walker.add(step);
travelled += walk_step;
}
return GodRaysResult(inside, travelled);
}
void SpotLight::draw(const core::visual::VisualParams* vparams)
{
float zNear, zFar;
computeClippingPlane(vparams, zNear, zFar);
Vector3 dir = d_direction.getValue();
if (d_lookat.getValue())
dir -= d_position.getValue();
computeOpenGLProjectionMatrix(m_lightMatProj, m_shadowTexWidth, m_shadowTexHeight, 2 * d_cutoff.getValue(), zNear, zFar);
computeOpenGLModelViewMatrix(m_lightMatModelview, d_position.getValue(), dir);
if (d_drawSource.getValue() && vparams->displayFlags().getShowVisualModels())
{
float baseLength = zFar * tanf(this->d_cutoff.getValue() * M_PI / 180);
float tipLength = (baseLength*0.5) * (zNear/ zFar);
Vector3 direction;
if(d_lookat.getValue())
direction = this->d_direction.getValue() - this->d_position.getValue();
else
direction = this->d_direction.getValue();
direction.normalize();
Vector3 base = this->getPosition() + direction*zFar;
Vector3 tip = this->getPosition() + direction*zNear;
std::vector<Vector3> centers;
centers.push_back(this->getPosition());
vparams->drawTool()->setPolygonMode(0, true);
vparams->drawTool()->setLightingEnabled(false);
vparams->drawTool()->drawSpheres(centers, zNear*0.1,d_color.getValue());
vparams->drawTool()->drawCone(base, tip, baseLength, tipLength, d_color.getValue());
vparams->drawTool()->setLightingEnabled(true);
vparams->drawTool()->setPolygonMode(0, false);
}
}
void Matrix4::rotate(double angle, Vector3 &a) {
Vector3 b = Vector3(a.getX(), a.getY(), a.getZ());
b.normalize();
m[0][0] = b.x*b.x + cos(angle)*(1-b.x*b.x);
m[0][1] = b.x*b.y*(1-cos(angle)) - b.z*sin(angle);
m[0][2] = b.x*b.z*(1-cos(angle)) + b.y*sin(angle);
m[0][3] = 0;
m[1][0] = b.x*b.y*(1-cos(angle)) + b.z*sin(angle);
m[1][1] = b.y*b.y + cos(angle)*(1-b.y*b.y);
m[1][2] = b.y*b.z*(1-cos(angle)) - b.x*sin(angle);
m[1][3] = 0;
m[2][0] = b.x*b.z*(1-cos(angle)) - b.y*sin(angle);
m[2][1] = b.y*b.z*(1-cos(angle)) + b.x*sin(angle);
m[2][2] = b.z*b.z + cos(angle)*(1-b.z*b.z);
m[2][3] = 0;
m[3][0] = 0;
m[3][1] = 0;
m[3][2] = 0;
m[3][3] = 1;
}
vector<Pin*> PinLineCurve::GetLinePins( Point3 start, Point3 end )
{
vector<Pin*> rtn;
Vector3 v = end-start;
int numSteps = v.getLength()/.55;
v.normalize();
v = .6f * v;
Point3 c = start;
rtn.push_back(new Pin(c));
for (int i = 0;i<numSteps;i++)
{
rtn.push_back(new Pin(c+v));
c = c+v;
}
/*Vector3 v2 = end-c;
float rest = v2.getLength();
if (rest!=0)
{
rtn.push_back(new Pin(c));
}*/
return rtn;
}
/***
* Calculates the intersection
* origin : point of origin for the eye
* dir : direction the ray is facing
* returns true if intersect, false if it does not.
***/
Point Sphere::intersect( Point3 origin, Vector3 dir ){
dir.normalize(); // Now A = 1.
float B = 2 * ( dir.x * ( origin.x - _x ) + dir.y * ( origin.y - _y ) + dir.z * (origin.z - _z ) );
float C = (origin.x - _x) * (origin.x - _x ) + (origin.y - _y)*(origin.y - _y) + (origin.z - _z)*(origin.z - _z) - _r *_r;
float w = (B * B)- 4 * C;
// If the point intersects the sphere.
if( w == 0 ){
w = -B/2;
// Store the point of intersection for later use.
pt_intersect = Point3( origin.x + dir.x * w, origin.y + dir.y * w, origin.z + dir.z * w );
// Return a point with the intersection, surface normal, material's red, green, and blue values, and light exponent.
return Point( pt_intersect, (pt_intersect - Point3( _x, _y, _z)), _red, _green, _blue, l_exponent, kr, kt);
}
// If the point intersects the sphere twice.
else if( w > 0 ){
float w1 = -(B + sqrt( w)) / 2;
float w2 = -(B - sqrt( w )) / 2;
// The least positive point is the point we wish to use.
if( w1 > w2 ){
pt_intersect = Point3( origin.x + dir.x * w2, origin.y + dir.y * w2, origin.z + dir.z * w2 );
}
else {
pt_intersect = Point3( origin.x + dir.x * w1, origin.y + dir.y * w1, origin.z + dir.z * w1 );
}
// Return a point with the intersection, surface normal, material's red, green, and blue values, and light exponent.
return Point( pt_intersect, (pt_intersect - Point3( _x, _y, _z)), _red, _green, _blue, l_exponent, kr, kt);
}
// There is no intersection, return an empty point.
return Point();
}
void Camera::move(float dx, float dy, float dz)
{
// Moves the camera by dx world units to the left or right; dy
// world units upwards or downwards; and dz world units forwards
// or backwards.
if (m_behavior == CAMERA_BEHAVIOR_ORBIT)
{
// Orbiting camera is always positioned relative to the
// target position. See updateViewMatrix().
return;
}
Vector3 eye = m_eye;
Vector3 forwards;
if (m_behavior == CAMERA_BEHAVIOR_FIRST_PERSON)
{
// Calculate the forwards direction. Can't just use the camera's local
// z axis as doing so will cause the camera to move more slowly as the
// camera's view approaches 90 degrees straight up and down.
forwards = Vector3::cross(WORLD_YAXIS, m_xAxis);
forwards.normalize();
}
else
{
forwards = m_viewDir;
}
eye += m_xAxis * dx;
eye += WORLD_YAXIS * dy;
eye += forwards * dz;
setPosition(eye);
}
bool Planet::get_color(ColorRGBA& resulting_color, const Vector2& screen, Math::Ray ray, gfloat& distance)const
{
if(sphere.radius<=0.f)
return false;
ray.transform(sphere.inv_transformation);
if(!sphere.intersects_local(ray, distance))
return false;
Vector3 p = ray.origin + ray.dir*distance;
p.normalize();
p *= sphere.radius;
Vector3 normal = p;
normal *= 1.f/sphere.radius;
if(Manager::get_settings().get_dbg_normal())
{
resulting_color.set_direction(normal);
}else
{
Vector2 uv;
vec_to_uv(uv, normal);
if(Manager::get_settings().get_dbg_uv())
{
resulting_color.set(uv.x, uv.y, 0.f, 1.f);
}else
{
shader(resulting_color, screen, uv, normal, ray);
}
}
return true;
}
void RangeScanner::construct_laser_stripe(Vector3 _startPt, double _shift, double _laserFOV, LaserStripe& stripe) {
Vector3 laser_start = _startPt-laser_pos;
laser_start.normalize();
Vector3 baseline = sensor.camera_pos() - laser_pos;
baseline.normalize();
Vector3 rotation_axis = laser_start.cross(baseline);
rotation_axis.normalize();
// rotate laser start about rotation axis, by _shift amount and width _laserFOV
RotationMatrix rotation(rotation_axis, _shift);
Vector3 rotated_laser_centered = rotation.rotate(laser_start);
RotationMatrix minus_rotation(rotation_axis, _shift-0.5*_laserFOV);
Vector3 rotated_laser_minus = minus_rotation.rotate(laser_start);
RotationMatrix plus_rotation(rotation_axis, _shift+0.5*_laserFOV);
Vector3 rotated_laser_plus = plus_rotation.rotate(laser_start);
//cout << "laser stripe: " << rotated_laser_minus << " : " << rotated_laser_centered << " : " << rotated_laser_plus << endl;
// obtain laser stripe planes
//Vector3 minus_n = rotated_laser_minus.cross(rotation_axis);
Vector3 minus_n = rotation_axis.cross(rotated_laser_minus);
minus_n.normalize();
double minus_d = -(minus_n.dotProduct(laser_pos));
//Vector3 plus_n = rotation_axis.cross(rotated_laser_plus);
Vector3 plus_n = rotated_laser_plus.cross(rotation_axis);
plus_n.normalize();
double plus_d = -(plus_n.dotProduct(laser_pos));
Vector3 centered_n = rotated_laser_centered.cross(rotation_axis);
centered_n.normalize();
double centered_d = -(centered_n.dotProduct(laser_pos));
stripe = LaserStripe(minus_n, minus_d, centered_n, centered_d, plus_n, plus_d);
}
//<<<<<<<<<<<<<<<<<<<<<<<两球碰撞>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
bool Ball::isIntersect(Ball* ball, Vector3& n){
double sqDist = SqDistPointBall(c, *ball);
bool isHit = ((sqDist <= (ball->radius+radius)*(ball->radius+radius)) ? true : false);
if(isHit){
n = c - ball->c;
n.normalize();
if(isCollinear(n, ball->speed) && isCollinear(n, speed)){//两速度向量与两球球心连线共线
Vector3 u1, u2;
u1 = speed - (1.0 + e) * ball->mass * (speed - ball->speed) / (mass + ball->mass);
u2 = ball->speed + (1.0 + e) * mass * (speed - ball->speed) / (mass + ball->mass);
speed = u1;
ball->speed = u2;
}
else {//斜碰
Vector3 vt1, vt2, vn1, vn2, ut1, ut2, un1, un2; //切线速度，法线速度
double cosine; //余弦
cosine = dot(speed, -n) / speed.length();
vn1 = speed.length()*cosine*(-n);
vt1 = speed - vn1;
if (ball->speed.length() >0.001) {
cosine = dot(ball->speed,n) / ball->speed.length();
vn2 = ball->speed.length()*cosine*n;
vt2 = ball->speed - vn2;
}
ut1 = vt1;
ut2 = vt2;
un1 = vn1 - (1.0 + e)*ball->mass*(vn1-vn2)/(mass+ball->mass);
un2 = vn2 + (1.0 + e)*mass*(vn1-vn2)/(mass+ball->mass);
speed = ut1 + un1;
ball->speed = ut2 + un2;
}
}
return false;
}
Matrix4
Matrix4::rotation(float angle, const Vector3 &vector)
{
Matrix4 m;
float c = cosf(angle);
float ci = 1.0f - c;
float s = sinf(angle);
Vector3 v = vector.normalize();
float xy = v.x * v.y * ci;
float xz = v.x * v.z * ci;
float yz = v.y * v.z * ci;
float xs = v.x * s;
float ys = v.y * s;
float zs = v.z * s;
m[0] = v.x * v.x * ci + c;
m[1] = xy + zs;
m[2] = xz - ys;
// m[3] = 0.0f;
m[4] = xy - zs;
m[5] = v.y * v.y * ci + c;
m[6] = yz + xs;
// m[7] = 0.0f;
m[8] = xz + ys;
m[9] = yz - xs;
m[10] = v.z * v.z * ci + c;
// m[11] = 0.0f;
// m[12] = 0.0f;
// m[13] = 0.0f;
// m[14] = 0.0f;
// m[15] = 1.0f;
return m;
}
void AltitudeMap::normalAtPoint(int x, int z, Vector3 &n)
{
// Returns the normal at the specified location on the height map.
// The normal is calculated using the properties of the height map.
// This approach is much quicker and more elegant than triangulating the
// height map and averaging triangle surface normals.
if (x > 0 && x < xsize - 1)
n.x = getAltitude(x - 1, z) - getAltitude(x + 1, z);
else if (x > 0)
n.x = 2.0f * (getAltitude(x - 1, z) - getAltitude(x, z));
else
n.x = 2.0f * (getAltitude(x, z) - getAltitude(x + 1, z));
if (z > 0 && z < ysize - 1)
n.z = getAltitude(x, z - 1) - getAltitude(x, z + 1);
else if (z > 0)
n.z = 2.0f * (getAltitude(x, z - 1) - getAltitude(x, z));
else
n.z = 2.0f * (getAltitude(x, z) - getAltitude(x, z + 1));
n.y = 2.0f * 16.0f;
n.normalize();
}
unsigned ParticleCableConstraint::addContact(ParticleContact * _contact, unsigned _limit) const
{
// find the length of the cable
real length = this->currentLength();
// check if we're over-extended
if (length < this->maxLength) {
return 0;
}
// otherwise return the contact
_contact->particle[0] = this->particle;
_contact->particle[1] = 0;
// calculate the normal
Vector3 normal = this->anchor - particle->getPosition();
normal.normalize();
_contact->contactNormal = normal;
_contact->penetration = length - this->maxLength;
_contact->restitution = this->restitution;
return 1;
}
void Matrix44::setFrontAndOrthonormalize(Vector3 front)
{
front.normalize();
//put the up vector in the matrix
m[8] = front.x;
m[9] = front.y;
m[10] = front.z;
//orthonormalize
Vector3 right,up;
right = rightVector();
if ( abs(right.dot( front )) < 0.99998 )
{
right = topVector().cross( front );
up = front.cross( right );
}
else
{
up = front.cross( right );
right = up.cross( front );
}
right.normalize();
up.normalize();
m[4] = up.x;
m[5] = up.y;
m[6] = up.z;
m[0] = right.x;
m[1] = right.y;
m[2] = right.z;
}
int Joint::addContact(CollisionData* data, int limit) const
{
if (data->isFull()) return 0;
Contact* contact = data->contacts;
// The position of each connection point in WCS
Vector3 a_pos_world = body[0]->getPointInWorldSpace(position[0]);
Vector3 b_pos_world;
if(body[1])
b_pos_world = body[1]->getPointInWorldSpace(position[1]);
else // body is NULL, then use the position[1] point be in WCS
{
b_pos_world = position[1];
contact->body[1] = NULL;
}
// Calculate the length of the joint
Vector3 a_to_b = b_pos_world - a_pos_world;
Vector3 relativePositionNormal = a_to_b;
relativePositionNormal.normalize();
real length = a_to_b.magnitude();
// Check if it is violated
if (real_abs(length) > epsilonDistance)
{
contact->body[0] = body[0];
contact->body[1] = body[1];
contact->contactNormal = relativePositionNormal;
contact->contactPoint = (a_pos_world + b_pos_world) * 0.5f;
contact->penetration = length - epsilonDistance;
contact->friction = 1.0f;
contact->restitution = 0;
data->addContacts(1);
return 1;
}
return 0;
}
Vector3 calcNormalC() {
Vector3 point1 = a-c;
Vector3 point2 = b-c;
normalC = point1.cross(point2);
normalC.normalize();
}
void normalize()
{
normal.normalize();
}
Vector3 calcNormalA() {
Vector3 point1 = b-a;
Vector3 point2 = c-a;
normalA = point1.cross(point2);
normalA.normalize();
}
Vector3 calcNormalB() {
Vector3 point1 = a-b;
Vector3 point2 = c-b;
normalB = point1.cross(point2);
normalB.normalize();
}
Vector3 PhotonTracer::refractDirection(Vector3& dir, Vector3& normal, float IOR)
{
Vector3 result;
bool TIR = false;
float n = 1.0f;
float nt = IOR;
float incidentDot = Vector3::DotProduct(dir, normal);
//entering the object
if(incidentDot < 0){
//refraction index
float compN = n / nt;
//compute the refracted direction
double c1, cs2;
c1 = -incidentDot;
cs2 = 1.0f - compN * compN * (1.0f - c1 * c1);
result = compN * dir + (compN * c1 - sqrt(cs2)) * normal;
result.normalize();
}
//exiting the glass
else{
//refraction index
float compN = nt / n;
double c1, cs2;
c1 = incidentDot;
cs2 = 1.0f - compN * compN * (1.0f - c1 * c1);
if(cs2 < 0.0f)
TIR = true;
if(!TIR){
//compute the refracted direction
result = compN * dir - (compN * c1 - sqrt(cs2)) * normal;
result.normalize();
}
}
if(TIR){
Vector3 negativeNormal = -normal;
return reflectDirection(dir, negativeNormal);
}
float cos1, cos2;
if(incidentDot < 0){
cos1 = -incidentDot;
cos2 = -Vector3::DotProduct(normal, result);
}
else{
cos1 = incidentDot;
cos2 = Vector3::DotProduct(normal, result);
}
float parallel = (nt * cos1 - n * cos2) / (nt * cos1 + n * cos2);
float perp = (n * cos1 - nt * cos2) / (n * cos1 + nt * cos2);
float reflectComp = 0.5f * (parallel * parallel + perp * perp);
float randomVal = (float)(rand() % 1025) / 1024.0f;
if(randomVal <= reflectComp){
if(incidentDot < 0.0f)
return reflectDirection(dir, normal);
else{
Vector3 negativeNormal = -normal;
return reflectDirection(dir, negativeNormal);
}
}
else
return result;
}
bool InverseKinematics::calcElbowPosition(Vector3<float> &target, const Vector3<float> &targetDir, int side, Vector3<float> &elbow, const RobotDimensions& theRobotDimensions) {
const Vector3<float> M1(0, 0, 0); //shoulder
const Vector3<float> M2(target); //hand
const float r1 = theRobotDimensions.values_[RobotDimensions::upperArmLength];
const float r2 = theRobotDimensions.values_[RobotDimensions::lowerArmLength];
const Vector3<float> M12 = M2 - M1;
Vector3<float> n = target;
n.normalize();
//center of intersection circle of spheres around shoulder and hand
const Vector3<float> M3 = M1 + M12 * ((sqr(r1) - sqr(r2)) / (2 * M12.squareAbs()) + 0.5f);
//calculate radius of intersection circle
const Vector3<float> M23 = M3 - M2;
float diff = sqr(r2) - M23.squareAbs();
const float radius = sqrt(diff);
//determine a point on the circle
const bool specialCase = n.x == 1 && n.y == 0 && n.z == 0 ? true : false;
const Vector3<float> bla(specialCase ? 0.0f : 1.0f, specialCase ? 1.0f : 0.0f, 0.0f);
const Vector3<float> pointOnCircle = M3 + (n ^ bla).normalize(radius);
//find best point on circle
float angleDiff = M_PI * 2.0f / 3.0f;
Vector3<float> bestMatch = pointOnCircle;
float bestAngle = 0.0f;
float newBestAngle = bestAngle;
float bestQuality = -2.0f;
Vector3<float> tDir = targetDir;
tDir.normalize();
const int offset = side == 1 ? 0 : 4;
int iterationCounter = 0;
const float maxAngleEpsilon = 1.0f * M_PI / 180.0f;
while(2.0f * angleDiff > maxAngleEpsilon)
{
for(int i = -1; i <= 1; i++)
{
if(i == 0 && iterationCounter != 1)
continue;
iterationCounter++;
const Pose3D elbowRotation(RotationMatrix(n, bestAngle + angleDiff * i));
const Vector3<float> possibleElbow = elbowRotation * pointOnCircle;
const Vector3<float> elbowDir = (M3 - possibleElbow).normalize();
float quality = elbowDir * tDir;
if(quality > bestQuality)
{
bestQuality = quality;
bestMatch = possibleElbow;
newBestAngle = bestAngle + angleDiff * i;
}
}
angleDiff /= 2.0f;
bestAngle = newBestAngle;
}
//printf("iterations %d\n", iterationCounter);
if(bestQuality == -2.0f)
return false;
//special case of target-out-of-joints-limit problem
float tAJR[NUM_JOINTS];
calcJointsForElbowPos(bestMatch, target, tAJR, offset, theRobotDimensions);
int jointInd = LShoulderPitch + offset + 1;
float minVal = robot_joint_signs[jointInd] * minJointLimits[jointInd];
if(tAJR[offset + 1] < minVal)
{
tAJR[offset + 1] = minVal;
Pose3D shoulder2Elbow;
shoulder2Elbow.translate(0, -theRobotDimensions.values_[RobotDimensions::upperArmLength], 0);
shoulder2Elbow.rotateX(-(tAJR[offset + 1] - M_PI_2));
shoulder2Elbow.rotateY(tAJR[offset + 0] + M_PI_2);
Vector3<float> handInEllbow = shoulder2Elbow * target;
handInEllbow.normalize(theRobotDimensions.values_[RobotDimensions::lowerArmLength]);
target = shoulder2Elbow.invert() * handInEllbow;
bestMatch = shoulder2Elbow.invert() * Vector3<float>(0, 0, 0);
}
elbow = bestMatch;
return true;
}
void MobileVRInterface::set_position_from_sensors() {
_THREAD_SAFE_METHOD_
// this is a helper function that attempts to adjust our transform using our 9dof sensors
// 9dof is a misleading marketing term coming from 3 accelerometer axis + 3 gyro axis + 3 magnetometer axis = 9 axis
// but in reality this only offers 3 dof (yaw, pitch, roll) orientation
uint64_t ticks = OS::get_singleton()->get_ticks_usec();
uint64_t ticks_elapsed = ticks - last_ticks;
float delta_time = (double)ticks_elapsed / 1000000.0;
// few things we need
Input *input = Input::get_singleton();
Vector3 down(0.0, -1.0, 0.0); // Down is Y negative
Vector3 north(0.0, 0.0, 1.0); // North is Z positive
// make copies of our inputs
bool has_grav = false;
Vector3 acc = input->get_accelerometer();
Vector3 gyro = input->get_gyroscope();
Vector3 grav = input->get_gravity();
Vector3 magneto = scale_magneto(input->get_magnetometer()); // this may be overkill on iOS because we're already getting a calibrated magnetometer reading
if (sensor_first) {
sensor_first = false;
} else {
acc = scrub(acc, last_accerometer_data, 2, 0.2);
magneto = scrub(magneto, last_magnetometer_data, 3, 0.3);
};
last_accerometer_data = acc;
last_magnetometer_data = magneto;
if (grav.length() < 0.1) {
// not ideal but use our accelerometer, this will contain shakey shakey user behaviour
// maybe look into some math but I'm guessing that if this isn't available, its because we lack the gyro sensor to actually work out
// what a stable gravity vector is
grav = acc;
if (grav.length() > 0.1) {
has_grav = true;
};
} else {
has_grav = true;
};
bool has_magneto = magneto.length() > 0.1;
if (gyro.length() > 0.1) {
/* this can return to 0.0 if the user doesn't move the phone, so once on, it's on */
has_gyro = true;
};
if (has_gyro) {
// start with applying our gyro (do NOT smooth our gyro!)
Basis rotate;
rotate.rotate(orientation.get_axis(0), gyro.x * delta_time);
rotate.rotate(orientation.get_axis(1), gyro.y * delta_time);
rotate.rotate(orientation.get_axis(2), gyro.z * delta_time);
orientation = rotate * orientation;
tracking_state = ARVRInterface::ARVR_NORMAL_TRACKING;
};
///@TODO improve this, the magnetometer is very fidgity sometimes flipping the axis for no apparent reason (probably a bug on my part)
// if you have a gyro + accelerometer that combo tends to be better then combining all three but without a gyro you need the magnetometer..
if (has_magneto && has_grav && !has_gyro) {
// convert to quaternions, easier to smooth those out
Quat transform_quat(orientation);
Quat acc_mag_quat(combine_acc_mag(grav, magneto));
transform_quat = transform_quat.slerp(acc_mag_quat, 0.1);
orientation = Basis(transform_quat);
tracking_state = ARVRInterface::ARVR_NORMAL_TRACKING;
} else if (has_grav) {
// use gravity vector to make sure down is down...
// transform gravity into our world space
grav.normalize();
Vector3 grav_adj = orientation.xform(grav);
float dot = grav_adj.dot(down);
if ((dot > -1.0) && (dot < 1.0)) {
// axis around which we have this rotation
Vector3 axis = grav_adj.cross(down);
axis.normalize();
Basis drift_compensation(axis, acos(dot) * delta_time * 10);
orientation = drift_compensation * orientation;
};
};
// JIC
orientation.orthonormalize();
last_ticks = ticks;
};
void QuadTree::CheckCollisionsNode(MyNode* n)
{
if (n->bl != NULL)
{
CheckCollisionsNode(n->bl);
CheckCollisionsNode(n->tl);
CheckCollisionsNode(n->br);
CheckCollisionsNode(n->tr);
return;
}
else if (!n->hasBall)
{
return;
}
else if(n->myObjects.size()<2 && !n->hasWall)//Only contains the ball
{
return;
}
// check collisions with objects
for (int i = 0;i<n->myObjects.size();i++)
{
if ((n->myObjects)[i]->objectType != GameObject::type::BALL)
{
continue;
}
for (int j = 0;j<n->myObjects.size();j++)
{
if (i == j)// Same object
continue;
Vector3 diff = (n->myObjects)[i]->position-(n->myObjects)[j]->position;
float minDist = (n->myObjects)[i]->radius+(n->myObjects)[j]->radius;
if (diff.getLength()<minDist)
{
//////////////////////////////////////////////////////////////////////////
// Ball
//////////////////////////////////////////////////////////////////////////
if ((n->myObjects)[j]->objectType == GameObject::type::BALL)
{
Vector3 newV = diff;
newV.normalize();
Vector3 tmpV = minDist * newV;
(n->myObjects)[i]->Position((n->myObjects)[j]->position + tmpV);
newV = ((Ball*)(n->myObjects)[i])->Velocity().getLength() * .7 * newV;
((Ball*)(n->myObjects)[i])->Velocity(newV);
//playFile("Sounds//BallHit.wav");
}
//////////////////////////////////////////////////////////////////////////
// Pin
//////////////////////////////////////////////////////////////////////////
else if ((n->myObjects)[j]->objectType == GameObject::type::PIN)
{
Vector3 newV = diff;
newV.normalize();
Vector3 tmpV = minDist * newV;
(n->myObjects)[i]->Position((n->myObjects)[j]->position + tmpV);
newV = ((Ball*)(n->myObjects)[i])->Velocity().getLength() * .7 * newV;
// to prevent the balls from getting stuck on the top of pins, we add just a little to the x in velocity.
newV = newV + Vector3(.001,0,0);
((Ball*)(n->myObjects)[i])->Velocity(newV);
//playFile("Sounds//PinHit.wav");
}
//////////////////////////////////////////////////////////////////////////
// Spinner
//////////////////////////////////////////////////////////////////////////
else if ((n->myObjects)[j]->objectType == GameObject::type::SPINNER)
{
Ball* b = (Ball*)(n->myObjects)[i];
Spinner* s = (Spinner*)(n->myObjects)[j];
Plane bc1 = Plane(s->boundingCube[0],s->boundingCube[1],s->boundingCube[4]);
Plane bc2 = Plane(s->boundingCube[1],s->boundingCube[2],s->boundingCube[5]);
Plane bc3 = Plane(s->boundingCube[2],s->boundingCube[3],s->boundingCube[6]);
Plane bc4 = Plane(s->boundingCube[3],s->boundingCube[0],s->boundingCube[7]);
// Check within bounding box
Vector3 pdiff = (b->Position()-bc1.ClosestPointToPoint(b->Position()));
if (pdiff.getLength()<= b->radius || bc1.normal * pdiff <= 0)
{
pdiff = (b->Position()-bc2.ClosestPointToPoint(b->Position()));
if (pdiff.getLength()<= b->radius || bc2.normal * pdiff <= 0)
{
pdiff = (b->Position()-bc3.ClosestPointToPoint(b->Position()));
if (pdiff.getLength()<= b->radius || bc3.normal * pdiff <= 0)
{
pdiff = (b->Position()-bc4.ClosestPointToPoint(b->Position()));
if (pdiff.getLength()<= b->radius || bc4.normal * pdiff <= 0)
{
// Check which quadrant withing the spinner the ball is in
Vector3 vTemp = (Vector3&)(b->position - s->boundingCube[0]);
float dist = vTemp.getLength();
int index = 0;
for (int i=1;i < 8;i++)
{
Vector3 v = (Vector3&)(b->position - s->boundingCube[i]);
float newDist = v.getLength();
if(dist > newDist)
{
dist = newDist;
index = i;
}
}
if (index == 0 || index == 4)
{
//[planes 1 2 3 4
CheckWalls(s->Planes()[0],s->Planes()[1],s->Planes()[2],s->Planes()[3], b,s);
}
else if (index == 1 || index == 5)
{
//planes 3 4 5 6
CheckWalls(s->Planes()[3],s->Planes()[4],s->Planes()[5],s->Planes()[6], b,s);
}
else if (index == 2 || index == 6)
{
//planes 6 7 8 9
CheckWalls(s->Planes()[6],s->Planes()[7],s->Planes()[8],s->Planes()[9], b,s);
}
else if (index == 3 || index == 7)
{
//planes 9 10 11 0
CheckWalls(s->Planes()[9],s->Planes()[10],s->Planes()[11],s->Planes()[0], b,s);
}
else
{
printf("There was a problem somewhere, I don't know what is even going on!");
}
}
}
}
}
}
//////////////////////////////////////////////////////////////////////////
// BALLHOLE
//////////////////////////////////////////////////////////////////////////
else if ((n->myObjects)[j]->objectType == GameObject::type::HOLE)
{
Ball *b = (Ball*)(n->myObjects)[i];
BallHole *h = (BallHole*)(n->myObjects)[j];
Vector3 vTemp = diff;
vTemp.normalize();
float hRad = vTemp.y*h->width + vTemp.x*h->height;
if (hRad + b->radius <= diff.getLength())
{
b->toDelete = true;
}
}
}
}
}
// check collisions with walls!
if (n->hasWall)
{
for (int i = 0;i<n->myObjects.size();i++)
{
if ((n->myObjects)[i]->objectType == GameObject::type::BALL)
{
Ball* b = (Ball*)(n->myObjects)[i];
for (int j = 0;j<n->wallPoints.size()-1;j++)
{
Vector3 wall = *(n->wallPoints)[j+1] - *(n->wallPoints)[j];
/*
// get position of ball, relative to line
var x1:Number = ball.x - line.x;
var y1:Number = ball.y - line.y;
*/
Vector3 ballFromWall = b->position - *(n->wallPoints)[j];
// check to see if the ball will collide with line
Vector3 rej = rejection(ballFromWall,wall);
if (rej.getLength() <= b->radius)
{
// check to see if the ball is above or below the line and check to see if it is heading towards or away from line
Plane p = Plane(*(n->wallPoints)[j], *(n->wallPoints)[j+1], Point3(0,0,1));
float relativePosition = p.normal * ballFromWall;
float relativeDirection = p.normal * b->Velocity();
if ((relativePosition > 0 && relativeDirection < 0) || (relativePosition < 0 && relativeDirection > 0))
{
/*
// get angle, sine and cosine
var angle:Number = line.rotation * Math.PI / 180;
var cos:Number = Math.cos(angle);
var sin:Number = Math.sin(angle);
*/
float theta, c, s;
theta = acos((wall * Vector3(1,0,0))/wall.getLength());
if (b->position.x<0)
{
theta *= -1;
}
c = cos(theta), s = sin(theta);
/*
// Original ActionScript code
// rotate coordinates
var y2:Number = cos * y1 - sin * x1;
// rotate velocity
var vy1:Number = cos * ball.vy - sin * ball.vx;
// rotate coordinates
var x2:Number = cos * x1 + sin * y1;
// rotate velocity
var vx1:Number = cos * ball.vx + sin * ball.vy;
y2 = -ball.height / 2;
vy1 *= bounce;
// rotate everything back;
x1 = cos * x2 - sin * y2;
y1 = cos * y2 + sin * x2;
ball.vx = cos * vx1 - sin * vy1;
ball.vy = cos * vy1 + sin * vx1;
ball.x = line.x + x1;
ball.y = line.y + y1;
*/
Vector3 position2;
Vector3 velocityRot;
position2 = Vector3(c * ballFromWall.x + s * ballFromWall.y,
c * ballFromWall.y - s * ballFromWall.x,
0);
velocityRot = Vector3( c * b->Velocity().x + s * b->Velocity().y,
c * b->Velocity().y - s * b->Velocity().x,
0);
// set the ball outside the wall
position2.y = b->radius;
velocityRot.y *= -1;
ballFromWall = Vector3(c * position2.x - s * position2.y,c * position2.y + s * position2.x,0);
b->Velocity(Vector3(c * velocityRot.x - s * velocityRot.y,c * velocityRot.y + s * velocityRot.x,0));
b->position = *(n->wallPoints)[j] + ballFromWall;
}
}
//delete(rej);
//// get the projection!
//Vector3 tmp = v;
//tmp.normalize();
//// get that funky projection, white boy
//Vector3 proj = *project(v2,tmp);
//// if the ball is in front of the start and behind the end, then it could collide.
//if (v*v2 >0 && proj.getLength()< v.getLength())
//{
// Plane plane = Plane(*n->wallPoints[j],*n->wallPoints[j+1],Point3((*n->wallPoints[j]).x,(*n->wallPoints[j]).y,-1));
// float rejections = rejection(v2, tmp)->getLength();
// if (rejections <= b->radius)
// {
// b->position = *n->wallPoints[j] + proj + (b->radius * 1.1f * plane.normal);
//
// float vel = b->Velocity().getLength();
// // COSS IS WRONG, FIX THIS THING!
// //float coss = (Vector3(1,0,0) * plane.normal);
// float coss = v * v2 * 1.0f/(v.getLength()*v2.getLength());
// float theta = acos(coss);
// /*if (b->position.x>0)
// {
// theta = acos(coss) + 3.14159/2.0;
// }
// else{
// theta = acos(coss) - 3.14159/2.0;
// }*/
// float c = cos(theta), s = sin(theta);
// Vector3 velP = c * b->Velocity(), velN = -1 * s * b->Velocity();
// float vpx = c * vel - s * velN.getLength(), vpy = s * vel + c*velN.getLength();
//
// Vector3 newv = Vector3(vpx, vpy, 0);
// newv.normalize();
//
// b->Velocity(vel * newv);
// }
//}
}
}
}
}
}
void CameraFlyer::update()
{
bool goingForward = gVirtualInput().isButtonHeld(mMoveForward);
bool goingBack = gVirtualInput().isButtonHeld(mMoveBack);
bool goingLeft = gVirtualInput().isButtonHeld(mMoveLeft);
bool goingRight = gVirtualInput().isButtonHeld(mMoveRight);
bool fastMove = gVirtualInput().isButtonHeld(mFastMove);
bool camRotating = gVirtualInput().isButtonHeld(mRotateCam);
if (camRotating != mLastButtonState)
{
if (camRotating)
Cursor::instance().hide();
else
Cursor::instance().show();
mLastButtonState = camRotating;
}
float frameDelta = gTime().getFrameDelta();
if (camRotating)
{
mYaw += Degree(gVirtualInput().getAxisValue(mHorizontalAxis) * ROTATION_SPEED * frameDelta);
mPitch += Degree(gVirtualInput().getAxisValue(mVerticalAxis) * ROTATION_SPEED * frameDelta);
mYaw = wrapAngle(mYaw);
mPitch = wrapAngle(mPitch);
Quaternion yRot;
yRot.fromAxisAngle(Vector3::UNIT_Y, Radian(mYaw));
Quaternion xRot;
xRot.fromAxisAngle(Vector3::UNIT_X, Radian(mPitch));
Quaternion camRot = yRot * xRot;
camRot.normalize();
SO()->setRotation(camRot);
}
Vector3 direction = Vector3::ZERO;
if (goingForward) direction += SO()->getForward();
if (goingBack) direction -= SO()->getForward();
if (goingRight) direction += SO()->getRight();
if (goingLeft) direction -= SO()->getRight();
if (direction.squaredLength() != 0)
{
direction.normalize();
float multiplier = 1.0f;
if (fastMove)
multiplier = FAST_MODE_MULTIPLIER;
mCurrentSpeed = Math::clamp(mCurrentSpeed + ACCELERATION * frameDelta, START_SPEED, TOP_SPEED);
mCurrentSpeed *= multiplier;
}
else
{
mCurrentSpeed = 0.0f;
}
float tooSmall = std::numeric_limits<float>::epsilon();
if (mCurrentSpeed > tooSmall)
{
Vector3 velocity = direction * mCurrentSpeed;
SO()->move(velocity * frameDelta);
}
}
void Camera::rotate(const int direction){
#define sqr(x) (x*x)
//http://inside.mines.edu/~gmurray/ArbitraryAxisRotation/ArbitraryAxisRotation.html#fig:axisrot
//r46 meg tartalmaz egy kozvetlen kepletet, matrixok nelkul, de mivel nem mukodott, lecsereltem. Gyorsitasi lehetoseg.
const float eps = 0.1f;
Matrix4 T;
Matrix4 Rxz;
Matrix4 Rxz2z;
Matrix4 Rz;
float d1;
float d;
Vector3 rotv;
if(direction==ROTATE_LEFT || direction==ROTATE_RIGHT){
T = T.translate(-pos);
d1 = (sqrt(sqr(up.getX()) + sqr(up.getY()) ));
d = sqrt(sqr(up.getX())+sqr(up.getY())+sqr(up.getZ()));
if(d1>=eps){
Rxz.set(0,up.getX()/d1);
Rxz.set(1,up.getY()/d1);
Rxz.set(4,-up.getY()/d1);
Rxz.set(5,up.getX()/d1);
}
Rxz2z.set(0,up.getZ()/d);
Rxz2z.set(2,-d1/d);
Rxz2z.set(8,d1/d);
Rxz2z.set(10,up.getZ()/d);
if(direction==ROTATE_LEFT){
rotv.setZ(rotationIntensity);
}else{//ROTATE_RIGHT
rotv.setZ(-rotationIntensity);
}
Rz = Rz.rotate(rotv);
lookAt = lookAt*T*Rxz*Rxz2z*Rz*Rxz2z.invert()*Rxz.invert()*T.invert();
}else if(direction==LEAN_LEFT || direction==LEAN_RIGHT){
T = T.translate(-pos);
Vector3 dir(pos.getX()-lookAt.getX(), pos.getY()-lookAt.getY(), pos.getZ()-lookAt.getZ());
dir.normalize();
d1 = sqrt(sqr(dir.getX()) + sqr(dir.getY()) );
d = sqrt(sqr(dir.getX())+sqr(dir.getY())+sqr(dir.getZ()));
if(d1>=eps){
Rxz.set(0,dir.getX()/d1);
Rxz.set(1,dir.getY()/d1);
Rxz.set(4,-dir.getY()/d1);
Rxz.set(5,dir.getX()/d1);
}
Rxz2z.set(0,dir.getZ()/d);
Rxz2z.set(2,-d1/d);
Rxz2z.set(8,d1/d);
Rxz2z.set(10,dir.getZ()/d);
if(direction==LEAN_LEFT){
rotv.setZ(rotationIntensity);
}else{//LEAN_RIGHT
rotv.setZ(-rotationIntensity);
}
Rz = Rz.rotate(rotv);
up = up*T*Rxz*Rxz2z*Rz*Rxz2z.invert()*Rxz.invert()*T.invert();
}else if(ROTATE_UP || ROTATE_DOWN){
Vector3 dir = lookAt-pos;
dir.normalize();
Vector3 right = dir.crossProduct(up);
T = T.translate(-pos);
d1 = (sqrt(sqr(right.getX()) + sqr(right.getY()) ));
d = sqrt(sqr(right.getX())+sqr(right.getY())+sqr(right.getZ()));
if(d1>=eps){
Rxz.set(0,right.getX()/d1);
Rxz.set(1,right.getY()/d1);
Rxz.set(4,-right.getY()/d1);
Rxz.set(5,right.getX()/d1);
}
Rxz2z.set(0,right.getZ()/d);
Rxz2z.set(2,-d1/d);
Rxz2z.set(8,d1/d);
Rxz2z.set(10,right.getZ()/d);
if(direction==ROTATE_UP){
rotv.setZ(rotationIntensity);
}else{//ROTATE_DOWN
rotv.setZ(-rotationIntensity);
}
Rz = Rz.rotate(rotv);
Vector3 newLookAt = lookAt*T*Rxz*Rxz2z*Rz*Rxz2z.invert()*Rxz.invert()*T.invert();
dir = newLookAt-pos;
dir.normalize();
if(dir.crossProduct(up).length()>=0.2f){
lookAt = newLookAt;
}
//up = up *Rxz*Rxz2z*Rz*Rxz2z.invert()*Rxz.invert();
}
#undef sqr
}
void Ship::moveTime( double dtime ) {
bool rotateFlag=false;
if( dead ) {
deadTime += dtime;
if( deadTime > DEATH_TIME ) {
respawn();
}
return;
}
//aoe uhtnsao eutnaoheuntsaoheutnshoaeu
gravaccel = 0;
Vector3 gravV;
double gravD;
if( Flags::gravity_enabled ) {
for( vector<Planet>::const_iterator iter = Space::planets.begin();
iter != Space::planets.end();
iter++ ) {
gravV = iter->pos - pos; // vector from here to planet
if( gravV*gravV > TOLERANCE ) {
gravD = myG * mass * iter->mass / ( gravV * gravV ); // newtons law
gravV.normalize();
gravaccel += gravD * gravV;
//cerr << gravaccel << endl;
}
}
}
//=tnsaoheunsaohteutnsaoheutnshaoeu
if( (abs(accel(0) - 0) > TOLERANCE)
| (abs(accel(1) - 0) > TOLERANCE)
| (abs(accel(2) - 0) > TOLERANCE) )
{
// cerr << ':';
vel += dtime * accel;
}
if( (abs(gravaccel(0) - 0) > TOLERANCE)
| (abs(gravaccel(1) - 0) > TOLERANCE)
| (abs(gravaccel(2) - 0) > TOLERANCE) )
{
// cerr << ':';
vel += dtime * gravaccel;
}
if( (abs(vel(0) - 0) > TOLERANCE)
| (abs(vel(1) - 0) > TOLERANCE)
| (abs(vel(2) - 0) > TOLERANCE) )
{
// cerr << '.';
pos += dtime * vel;
for( int i = 0; i < 3; i++ ) {
if( pos(i) > Space::max(i) )
pos(i) = Space::min(i) + (pos(i) - Space::max(i));
else if( pos(i) < Space::min(i) )
pos(i) = Space::max(i) - (Space::min(i) - pos(i));
}
}
for( int i = 0; i<3; i++ )
if( abs(rotateRate(i) - 0) > TOLERANCE ) {
// cerr << ',';
double dangle;
Matrix33 m;
dangle = dtime * rotateRate(i);
switch( i ) {
case 0:
m = rotate( dangle, left );
accel = m * accel;
break;
case 1:
m = rotate( dangle, up );
accel = m * accel;
break;
case 2:
m = rotate( dangle, forwards );
// cerr << m << endl;
accel = m * accel;
break;
}
}
if( coolDown > 0 )
coolDown -= dtime;
// check against planets for collisions
/* for( vector<Planet>::const_iterator iter = Space::planets.begin();
iter != Space::planets.end();
iter++ ) {
double dist = iter->radius + radius;
gravV = iter->pos - pos;
if( gravV*gravV < dist*dist ) {
cerr << "Crashed:\n";
cerr << "Pos: " << pos << endl;
cerr << "Planet: " << iter->pos << endl;
cerr << "Radius: " << iter->radius << endl;
accel = vel = pos = Vector3();
}
}*/
// cerr << accel << endl;
// cerr << "pos: " << pos << endl;
// cerr << "up: " << up;// << endl;
// cerr << "forwards:" << forwards;
// cerr << "left:" << left;
// cerr << "rotateRate: " << rotateRate <<endl;
// cerr << "vel: " << vel << endl;
}
void Shot::moveTime( double dtime ) {
if( Flags::gravity_enabled ) {
Vector3 gravaccel = 0;
Vector3 gravV;
double gravD;
for( vector<Planet>::const_iterator iter = Space::planets.begin();
iter != Space::planets.end();
iter++ ) {
gravV = iter->pos - pos; // vector from here to planet
if( gravV*gravV > TOLERANCE ) {
gravD = myG * mass * iter->mass / ( gravV * gravV ); // newtons law
gravV.normalize();
gravaccel += gravD * gravV;
//cerr << gravaccel << endl;
}
}
}
if( (abs(vel(0) - 0) > TOLERANCE)
| (abs(vel(1) - 0) > TOLERANCE)
| (abs(vel(2) - 0) > TOLERANCE) )
{
Vector3 maxPos( pos );
Vector3 minPos( pos );
for( int i = 0; i < 3; i++ ) {
maxPos(i) += radius / 1.4;
minPos(i) -= radius / 1.4;
}
if( Flags::particles_enabled ) {
double part_rate;
if( Flags::low_quality )
part_rate = SHOT_PART_RATE_LOW;
else
part_rate = SHOT_PART_RATE;
Vector3 independentVel = vel - (SHOT_SPEED * forwards);
// cerr << independentVel << " aoeu " << vel << endl;
Space::particleDist( dtime * part_rate,
1.0, 1.0,
maxPos, minPos, independentVel, independentVel
);
}
pos += dtime * vel;
age += dtime;
for( int i = 0; i < 3; i++ ) {
if( pos(i) > Space::max(i) )
pos(i) = Space::min(i) + (pos(i) - Space::max(i));
else if( pos(i) < Space::min(i) )
pos(i) = Space::max(i) - (Space::min(i) - pos(i));
}
}
}
/***Use to change the coordinates of the vertices and draw the Graph ***/
void Change_Vertex(void){
/** Initial the zBuffer
**/
vector<vector<float>> zBuffer;
vector<float> temp_zBuffer;
for(int i=0;i<2000;i++)
{
temp_zBuffer.push_back(10000.0);
}
for(int j=0;j<2000;j++){
zBuffer.push_back(temp_zBuffer);
}
glClearColor(1.0, 1.0, 1.0,0.0);
/*********It is for file reading**********************/
ifstream f(F_PATH);
int numberOfVertices;
int numberOfPolygons;
int edegeOfPolyons;
string temp;
if(!f)
{
cout<<"No File!"<<endl;
return ;
}
string s;
getline(f, s);
istringstream is(s);
is>>temp;
is>>temp;
numberOfVertices=atoi(temp.c_str());
is>>temp;
numberOfPolygons=atoi(temp.c_str());
float vertex_list[numberOfVertices][4];
float edege_list[numberOfPolygons][20];
// float vertex_list[100000][4];
// float edege_list[100000][20];
for(int i=0;i<numberOfVertices;i++){
getline(f, s);
istringstream is(s);
is>>temp;
vertex_list[i][0]=atof(temp.c_str());
is>>temp;
vertex_list[i][1]=atof(temp.c_str());
is>>temp;
vertex_list[i][2]=atof(temp.c_str());
vertex_list[i][3]=1;
}
for(int i=0;i<=numberOfPolygons-1;i++){
getline(f,s);
istringstream is(s);
is>>temp;
edegeOfPolyons=atoi(temp.c_str());
int j=0;
for(j;j<=edegeOfPolyons-1;j++){
is>>temp;
edege_list[i][j]=atoi(temp.c_str());
}
for(j;j<=19;j++){
edege_list[i][j]=-1;
}
}
/**********file read ending *************************/
//draw the 3D Graph
Matrix4 finalMatrix=Integrated_Matrix();
int range=(sizeof(vertex_list)/sizeof(vertex_list[0][0])/4);
int edege_range=sizeof(edege_list)/sizeof(edege_list[0][0])/20-1;//
float end_Vertex[range][3];
Vector4 m;
Vector4 tempVector;
w=c-lookUpPoint;
w.normalize();
for(int i=0;i<range;i++){
m.x=vertex_list[i][0];
m.y=vertex_list[i][1];
m.z=vertex_list[i][2];
m.w=vertex_list[i][3];
tempVector=finalMatrix*m;
tempVector.x=tempVector.x/tempVector.w;
tempVector.y=tempVector.y/tempVector.w;
tempVector.z=tempVector.z/tempVector.w;
end_Vertex[i][0]=tempVector.x;
end_Vertex[i][1]=tempVector.y;
end_Vertex[i][2]=tempVector.z;//tempVector.z;
}
glClear(GL_COLOR_BUFFER_BIT);
int tag=0;
/*%%%%*/
float le[1000],re[1000],tempZ;
int i,j; int flag;/*determine the negative value*/
int overIndex; //when y1<0 and y2>0, try to make the fabs(y1),Howver,every y2 valus becomes y2+fabs(y1),so try this is the value of fabs(y1)
for(i=0;i<1000;i++)
le[i]=1000,re[i]=0;
/*%%%%*/
srand((unsigned)time(NULL));
for(int x=0;x<=edege_range;++x)
{
flag=0;
/**It is for hiding back face*****/
Vector3 temp;
Vector3 temp_a(vertex_list[(int)edege_list[x][1]-1][0]-vertex_list[(int)edege_list[x][0]-1][0],vertex_list[(int)edege_list[x][1]-1][1]-vertex_list[(int)edege_list[x][0]-1][1],vertex_list[(int)edege_list[x][1]-1][2]-vertex_list[(int)edege_list[x][0]-1][2]);
Vector3 temp_b(vertex_list[(int)edege_list[x][2]-1][0]-vertex_list[(int)edege_list[x][1]-1][0],vertex_list[(int)edege_list[x][2]-1][1]-vertex_list[(int)edege_list[x][1]-1][1],vertex_list[(int)edege_list[x][2]-1][2]-vertex_list[(int)edege_list[x][1]-1][2]);
Vector3 normal_vector=temp_a.crossProduct(temp_b);
temp.x=vertex_list[(int)edege_list[x][0]-1][0]-c.x;
temp.y=vertex_list[(int)edege_list[x][0]-1][1]-c.y;
temp.z=vertex_list[(int)edege_list[x][0]-1][2]-c.z;
if(normal_vector.dot(temp)<0){
continue;
}
/***end for hiding back face***/
/***use the OPENGL function to disply the graph***/
// glColor3f(0.0, 0.0, 0.0);
glBegin(GL_LINES);
//glBegin(GL_POINTS);
int j;
for(j=0;j<19;j++)
{
if(edege_list[x][j+1]<0){ //when reach the end of the edege array
break;
}
glVertex3fv(end_Vertex[(int)edege_list[x][j]-1]);
glVertex3fv(end_Vertex[(int)edege_list[x][j+1]-1]);
edgedetect(end_Vertex[(int)edege_list[x][j]-1][0],end_Vertex[(int)edege_list[x][j]-1][1],end_Vertex[(int)edege_list[x][j+1]-1][0],end_Vertex[(int)edege_list[x][j+1]-1][1],le,re,&flag,&overIndex);
}
glVertex3fv(end_Vertex[(int)edege_list[x][j]-1]);
glVertex3fv(end_Vertex[(int)edege_list[x][0]-1]);
edgedetect(end_Vertex[(int)edege_list[x][j]-1][0],end_Vertex[(int)edege_list[x][j]-1][1],end_Vertex[(int)edege_list[x][0]-1][0],end_Vertex[(int)edege_list[x][0]-1][1],le,re,&flag,&overIndex);
tempZ=end_Vertex[(int)edege_list[x][j]-1][2]*1000;
glEnd();
cout<<"flag:"<<flag<<endl;
cout<<"tempZ:"<<tempZ<<endl;
float red=(float)rand()/RAND_MAX;
float green=(float)rand()/RAND_MAX;
float blue=(float)rand()/RAND_MAX;
// cout<<"red:"<<red<<"green:"<<green<<"blue:"<<blue<<endl;
for(j=0;j<1000;j++)
{
if(le[j]<=re[j])
{
if(flag==0)
{
for(i=le[j];i<re[j];i++)
{
if(tempZ<=zBuffer[(int)i+1000][(int)j+1000])
{
draw_pixel(i,j,red,green,blue);
zBuffer[(int)i+1000][(int)j+1000]=tempZ;
}
}
}
if(flag==1)
{
for(i=le[j];i<re[j];i++)
{
if(tempZ<=zBuffer[(int)i+1000][(int)-j+1000])
{
draw_pixel(i,-j,red,green,blue);
zBuffer[(int)i+1000][(int)-j+1000]=tempZ;
}
}
}
if(flag==2)
{
if(j<overIndex)
{
for(i=le[j];i<re[j];i++)
{
if(tempZ<=zBuffer[(int)i+1000][(int)-j+1000])
{
draw_pixel(i,-j,red,green,blue);
zBuffer[(int)i+1000][(int)-j+1000]=tempZ;
}
}
}
else{
for(i=le[j];i<re[j];i++)
{
if(tempZ<=zBuffer[(int)i+1000][(int)j-overIndex+1000])
{
draw_pixel(i,j-overIndex,red,green,blue);
zBuffer[(int)i+1000][(int)j-overIndex+1000]=tempZ;
}
}
}
}
}
}
for(i=0;i<1000;i++)
le[i]=1000,re[i]=0;
}
glFlush();
}
