color calcReflectance(triple pt, int current) // Ray - 2*(Normal dot Ray)*Normal
{
triple center = {sphereArray[current].x, sphereArray[current].y, sphereArray[current].z};
triple normal = {0};
diff(&normal, pt, center);
triple ray = {0};
ray.x = pt.x + .0001 * normal.x;
ray.y = pt.y + .0001 * normal.y;
ray.z = pt.z + .0001 * normal.z;
//unitVector(&ray);
unitVector(&normal);
triple reflection = {0};
double scalar = 2*dotProduct(normal, ray);
reflection.x = ray.x - scalar*normal.x;
reflection.y = ray.y - scalar*normal.y;
reflection.z = ray.z - scalar*normal.z;
unitVector(&reflection);
double minMagnitude = 1000000;
int minSphere = -1;
int i;
for(i = 0; i < numspheres + 4; i++)
{
triple collision = relflectCollide(sphereArray[i], reflection, ray);
if(!isNull(collision) && magnitude(collision) < minMagnitude)
{
minMagnitude = magnitude(collision);
minSphere = i;
}
}
if(minSphere != -1)
{
triple collision = relflectCollide(sphereArray[minSphere], reflection, pt);
collision.x += pt.x;
collision.y += pt.y;
collision.z += pt.z;
double luminosity = calcShading(collision, minSphere);
color tempColor = sphereArray[minSphere].c;
if(minSphere == numspheres && !(((int)floor(collision.x*5)%2==0&&(int)floor(collision.z*5)%2!=0)||((int)floor(collision.x*5)%2!=0&&(int)floor(collision.z*5)%2==0)))
{
tempColor.r = (255-tempColor.r)*(.4 + .6*luminosity);
tempColor.g = (255-tempColor.g)*(.4 + .6*luminosity);
tempColor.b = (255-tempColor.b)*(.4 + .6*luminosity);
}
else
{
tempColor.r = tempColor.r*(.4 + .6*luminosity);
tempColor.g = tempColor.g*(.4 + .6*luminosity);
tempColor.b = tempColor.b*(.4 + .6*luminosity);
}
return tempColor;
}
else
{
return sphereArray[current].c;
}
}
inline Matrix viewMatrix(const Vector3& eye, const Vector3& gaze, const Vector3& up){
Matrix ret;
//Create an orthogonal basis from parameters
Vector3 w = -(unitVector(gaze));
Vector3 u = unitVector(cross(up, w));
Vector3 v = cross(w, u);
//Rotate orthogonal basis to xyz basis
ret.x[0][0] = u.x();
ret.x[0][1] = u.y();
ret.x[0][2] = u.z();
ret.x[1][0] = v.x();
ret.x[1][1] = v.y();
ret.x[1][2] = v.z();
ret.x[2][0] = w.x();
ret.x[2][1] = w.y();
ret.x[2][2] = w.z();
//Translate eye to xyz origin
Matrix move = identityMatrix();
move.x[0][3] = -eye.x();
move.x[1][3] = -eye.x();
move.x[2][3] = -eye.x();
ret *= move;
return ret;
}
double calcShading(triple pt, int current)
{
//printf("%f, %f, %f\n", pt.x, pt.y, pt.z);
triple center = {0};
center.x = sphereArray[current].x;
center.y = sphereArray[current].y;
center.z = sphereArray[current].z;
triple centerVector = {0};
diff(¢erVector, pt, center);
unitVector(¢erVector);
pt.x += .000001 * centerVector.x;
pt.y += .000001 * centerVector.y;
pt.z += .000001 * centerVector.z;
triple pointVector = {0};
diff(&pointVector, light, pt);
int i;
for(i = 0; i < NUMSPHERES; i++)
{
triple collision = shadeCollide(sphereArray[i], pt);
if(!isNull(collision))
return 0;
}
unitVector(&pointVector);
double result = dotProduct(pointVector, centerVector);
if(result > 0)
{
return result;
}
return 0;
}
void Arrow::adaptLine() const
{
// Use circle to represent zero vector (every vector shorter than zeroVectorTolerance
// is considered a zero vector)
if (mLength <= zeroVectorTolerance)
{
mLine = Shapes::toConvexShape(sf::CircleShape(mThickness));
mLine.setFillColor(mColor);
mLine.move(-mThickness, -mThickness);
}
// If the line length is shorter than the triangle height, don't draw the line
else if (mLength <= getTriangleHeight())
{
mLine = sf::ConvexShape();
}
// Normal arrow
else
{
sf::Vector2f arrowDirection = (mLength - getTriangleHeight()) * unitVector(mDirection);
mLine = Shapes::line(arrowDirection, mColor, getThickness());
}
}
bool MoveAnimation::onMove(sf::Time dt)
{
if (p_mNode == nullptr || mCoordinates.empty()) { return false; }
if (p_mNode->getPosition() == mCoordinates.front())
{
if (mCoordinates.size() > 1)
{
if (mTimeBuf >= mDuration)
{
mTimeBuf = 0.f;
mCoordinates.pop_front();
return false;
}
mTimeBuf -= dt.asSeconds();
}
else { return false; }
}
else
{
sf::Vector2f s = mCoordinates.front() - p_mNode->getPosition();
sf::Vector2f dir = unitVector(s);
p_mNode->setVelocity(dir * mSpeed);
if (magnitudeOf(p_mNode->getVelocity() * dt.asSeconds()) > magnitudeOf(s))
{
p_mNode->setVelocity(0.f, 0.f);
p_mNode->setPosition(mCoordinates.front());
if (mCoordinates.size() < 2) { mCoordinates.pop_front(); }
return false;
}
p_mNode->move(p_mNode->getVelocity() * dt.asSeconds());
}
return true;
}
//Rotation is in radian about an arbitrary axis
inline Matrix rotate(const Vector3& _axis, precision angle){
Vector3 axis = unitVector(_axis);
Matrix ret;
precision x = axis.x();
precision y = axis.y();
precision z = axis.z();
precision cosine = cos(angle);
precision sine = sin(angle);
precision t = 1 - cosine;
ret.x[0][0] = t * x * x + cosine;
ret.x[0][1] = t * x * y - sine * z;
ret.x[0][2] = t * x * z + sine * y;
ret.x[0][3] = 0.0f;
ret.x[1][0] = t * x * x + sine * z;
ret.x[1][1] = t * x * y + cosine;
ret.x[1][2] = t * x * z - sine * x;
ret.x[1][3] = 0.0f;
ret.x[2][0] = t * x * x - sine * y;
ret.x[2][1] = t * x * y + sine * x;
ret.x[2][2] = t * x * z + cosine;
ret.x[2][3] = 0.0f;
ret.x[3][0] = 0.0f;
ret.x[3][1] = 0.0f;
ret.x[3][2] = 0.0f;
ret.x[3][3] = 1.0f;
return ret;
}
triple willCollide(sphere sph, triple pix)
{
triple vector = {0};
diff(&vector, pix, eye);
unitVector(&vector);
double b = 2*(vector.x*(eye.x-sph.x)+vector.y*(eye.y-sph.y)+vector.z*(eye.z-sph.z));
double c = pow(eye.x-sph.x,2)+pow(eye.y-sph.y,2)+pow(eye.z-sph.z,2)-pow(sph.r,2);
double discriminant = pow(b,2) - 4*c;
if(discriminant >= 0)
{
double r1 = (-b - sqrt(pow(b,2)-4*c))/2.0;
double r2 = (-b + sqrt(pow(b,2)-4*c))/2.0;
triple ray = {0};
if(r1 < r2 && r1 > 0)
{
ray.x = eye.x + r1*vector.x;
ray.y = eye.y + r1*vector.y;
ray.z = eye.z + r1*vector.z;
return ray;
}
else if(r2 > 0)
{
ray.x = eye.x + r2*vector.x;
ray.y = eye.y + r2*vector.y;
ray.z = eye.z + r2*vector.z;
return ray;
}
return nulltrip;
}
else
return nulltrip;
}
void Samsquamptch::acquireTarget(sf::Vector2f target, sf::Vector2f bounds)
{
m_bounds = bounds;
m_target = direction(target);
m_velocity = unitVector(sf::Vector2f(m_target.x, m_target.y));
m_angle = std::atan2(m_velocity.y, m_velocity.x);
//std::cout<<"Target acquired: "<<m_target.x<<", "<<m_target.y<<std::endl;
}
Frame3D Frame3D::fromCompactAxisAngleRep(const boost::array<double, 6>& rep)
{
Frame3D retval;
retval.mAngleAxis = Eigen::AngleAxisd(rep[2], unitVector(rep[0], rep[1]));
retval.mPos = Vector3D(rep[3], rep[4], rep[5]);
return retval;
}
int boxCollide(sphere sph, triple pix)
{
triple vector = {0};
diff(&vector, light, pix);
unitVector(&vector);
double tFar = -1000000;
double tNear = 1000000;
return 0;
}
void Particle::initialise(float _a, float _v, float fric, sf::Vector2f grav, sf::Time lifeTime, sf::Vector2f pos, float scale, sf::Color col) {
dead = false;
force = sf::Vector2f(std::cos(_a) * _v, std::sin(_a) * _v);
friction = -unitVector(force) * fric;
gravity = sf::Vector2f(std::cos(grav.x) * grav.y, std::sin(grav.x) * grav.y);
startAlpha = col.a;
life = lifeTime;
position = pos;
color = col;
if(fric != 0) if(_v / fric < lifeTime.asSeconds()) life = sf::seconds(_v / fric);
}
/*PRIVATE FUNCTIONS*/
void Projectile::updateCurrent(sf::Time dt, CommandQueue& commands) {
if (isGuided()) {
const float approachRate = 200.f;
sf::Vector2f newVel = unitVector(approachRate * dt.asSeconds() * targetDirection + getVelocity());
newVel *= getMaxSpeed();
float angle = std::atan2(newVel.y, newVel.x);
setRotation(toDegree(angle) + 90.f);
setVelocity(newVel); //rotation and velocity set
}
Entity::updateCurrent(dt, commands); //rotation and velocity drawn on screen
}
void Foam::primitiveMesh::overrideCellCentres(const pointField& newCellCtrs)
{
Info << "Overriding spherical cell centres\n" << endl;
if (!cellCentresPtr_)
{
calcCellCentresAndVols();
}
vectorField& cellCtrs = *cellCentresPtr_;
forAll(cellCtrs, cellI)
{
cellCtrs[cellI] = unitVector(newCellCtrs[cellI])*mag(cellCtrs[cellI]);
}
make_elliptical_arc::
make_elliptical_arc( EllipticalArc& _ea,
curve_type const& _curve,
unsigned int _total_samples,
double _tolerance )
: ea(_ea), curve(_curve),
dcurve( unitVector(derivative(curve)) ),
model(), fitter(model, _total_samples),
tolerance(_tolerance), tol_at_extr(tolerance/2),
tol_at_center(0.1), angle_tol(0.1),
initial_point(curve.at0()), final_point(curve.at1()),
N(_total_samples), last(N-1), partitions(N-1), p(N)
{
}
sf::ConvexShape line(sf::Vector2f direction, const sf::Color& color, float thickness)
{
sf::Vector2f perpendicular = unitVector(perpendicularVector(direction)) * 0.5f * thickness;
sf::ConvexShape line;
line.setFillColor(color);
line.setPointCount(4);
line.setPoint(0, -perpendicular);
line.setPoint(1, perpendicular);
line.setPoint(2, direction + perpendicular);
line.setPoint(3, direction - perpendicular);
return line;
}
void Projectile::UpdateCurrent( sf::Time dt, CommandQueue& commands )
{
if ( IsGuided() )
{
const float approachRate = 200.f;
sf::Vector2f newVelocity = unitVector( approachRate * dt.asSeconds() * pImpl->mTargetDirection + GetVelocity() );
newVelocity *= GetMaxSpeed();
float angle = std::atan2( newVelocity.y, newVelocity.x );
setRotation( toDegree( angle ) + 90.f );
SetVelocity( newVelocity );
}
Entity::UpdateCurrent( dt, commands );
}
Texture::Visualization::Visualization(const Any& a) {
*this = Visualization();
if (a.type() == Any::ARRAY) {
if (a.nameEquals("bumpInAlpha")) {
*this = bumpInAlpha();
} else if (a.nameEquals("defaults")) {
*this = defaults();
} else if (a.nameEquals("linearRGB")) {
*this = linearRGB();
} else if (a.nameEquals("depthBuffer")) {
*this = depthBuffer();
} else if (a.nameEquals("packedUnitVector")) {
*this = packedUnitVector();
} else if (a.nameEquals("radiance")) {
*this = radiance();
} else if (a.nameEquals("reflectivity")) {
*this = reflectivity();
} else if (a.nameEquals("sRGB")) {
*this = sRGB();
} else if (a.nameEquals("unitVector")) {
*this = unitVector();
} else {
a.verify(false, "Unrecognized Visualization factory method");
}
} else {
a.verifyName("Texture::Visualization", "Visualization");
AnyTableReader r(a);
String c;
if (r.getIfPresent("channels", c)) {
channels = toChannels(c);
}
r.getIfPresent("documentGamma", documentGamma);
r.getIfPresent("invertIntensity", invertIntensity);
r.getIfPresent("max", max);
r.getIfPresent("min", min);
r.getIfPresent("layer", layer);
r.getIfPresent("mipLevel", mipLevel);
r.verifyDone();
}
}
bool HistoryDialog::onMove(sf::Time dt, sf::Vector2f dest)
{
if (getPosition() != dest)
{
sf::Vector2f s = dest - getPosition();
sf::Vector2f dir = unitVector(s);
setVelocity(dir * MOVEMENT_SPEED);
if (magnitudeOf(getVelocity() * dt.asSeconds()) > magnitudeOf(s))
{
setVelocity(0.f, 0.f);
setPosition(dest);
return true;
}
return false;
}
else
{
setVelocity(0.f, 0.f);
return true;
}
}
int main()
{
// Load: time, a.x, a.y, a.z from raw_data.csv
// Initialize variables
double timestamp, acceleration_x, acceleration_y, acceleration_z = 0;
int numLines = 0;
// Create file pointer
FILE *fp;
// Open stream to count lines in file
fp = fopen("raw_data.csv", "r");
int ch;
while (!feof(fp)){
ch = fgetc(fp);
if (ch == '\n'){
numLines ++;
}
}
numLines ++;
fclose(fp);
// Allocate space for array of times
// double *timeArray = malloc(sizeof(double) * numLines);
// Allocate space for array of acceleration
// double *accelerationArray = malloc(sizeof(double) * numLines * 3);
double timeArray[n];
double xyzAccelerationArray[m][n][3];
// Open stream to read the data
fp = fopen("raw_data.csv", "r");
char string[maxLength];
char* str;
int counter = 0;
int row;
int col;
while (fgets(string, maxLength, fp)) {
// Remove trailing \n
size_t ln = strlen(string) - 1;
if (string[ln] == '\n'){
string[ln] = '\0';
}
// Split the string by comma
str = _strdup(string);
char *element1 = strtok(str, ",");
char *element2 = strtok(NULL, ",");
str = _strdup(NULL);
char *element3 = strtok(str, ",");
char *element4 = strtok(NULL, ",");
// Change strings to doubles
timestamp = atof(element1);
acceleration_x = atof(element2);
acceleration_y = atof(element3);
acceleration_z = atof(element4);
// Store timestamps
timeArray[counter] = timestamp;
row = counter / n;
col = counter % n;
// Store acceleration
xyzAccelerationArray[row][col][0] = acceleration_x;
xyzAccelerationArray[row][col][1] = acceleration_y;
xyzAccelerationArray[row][col][2] = acceleration_z;
// printf("%s %s %s %s\n", element1, element2, element3, element4);
// printf("%f %f %f %f\n", timestamp, acceleration_x, acceleration_y, acceleration_z);
// printf("%s\n", string);
counter ++;
}
fclose(fp);
// End read from file
// Run Singular Variable Decomposition
double uMatrix[3][3];
double sMatrix[3][3];
svdcmp(xyzAccelerationArray, 3, numLines, uMatrix, sMatrix);
// Get U and S
Vector uVector = { uMatrix[0][0], uMatrix[0][1], uMatrix[0][2] };
Vector vVector = { uMatrix[1][0], uMatrix[1][1], uMatrix[1][2] };
Vector normalVector = { uMatrix[2][0], uMatrix[2][1], uMatrix[2][2] };
normalVector = unitVector(normalVector);
// Find intersection of vectors (0, 0, 0)
Point intersection = intersectionOfThreeVectors(uVector, vVector, normalVector);
// Create plane
Plane plane = { uVector, vVector, normalVector, intersection };
// Put points on to plane
// Find linear combination (rref)
// Return data
printf("\n");
printf("numLines: %d\n", numLines);
printf("end of file \n");
// printf("%f", timeArray[0]);
getchar();
return 0;
}
sf::Vector2f Samsquamptch::direction(sf::Vector2f target)
{
return unitVector(sf::Vector2f(target - getPosition()));
}
void Projectile::GuideTowards( sf::Vector2f position )
{
assert( IsGuided() );
pImpl->mTargetDirection = unitVector( position - GetWorldPosition() );
}
void keySpecialOperations()
{
if(keyStates['a'])
{
if(angley<=20)
angley+=0.4;
}
if(keyStates['d'])
{
if(angley>=-20)
angley-=0.4;
}
if(stop1==1)
{
t+=INCREMENT;
if(v<0)
{
goto label;
stop1=0;
}
if(v>=0)
{
v=u-FRICTIONAL_DECELERATION*t;
omega=omega0-ROTATIONAL_DECCELERATION*t;
theta=(omega0*t)+(0.5*(-ROTATIONAL_DECCELERATION)*t*t);
r=(u*t)+(0.5*(-FRICTIONAL_DECELERATION)*t*t);
}
unitVector(frontdirx,frontdiry,frontdirz);
x*=r;
y*=r;
z*=r;
carPositionx+=x;
carPositiony+=y;
carPositionz+=z;
anglex+=theta*(180.0/M_PI);
t-=INCREMENT;
u=v;
omega0=omega;
cameray=carPositiony+6.0f;
cameraz=carPositionz+13.0f;
referencez=carPositionz;
}
label:
if(stop2==1)
{
t+=INCREMENT;
if(v>0)
{
stop2=0;
goto label2;
}
if(v<0)
{
v=u+FRICTIONAL_DECELERATION*t;
omega=omega0+ROTATIONAL_DECCELERATION*t;
theta=(omega0*t)+(0.5*(ROTATIONAL_DECCELERATION)*t*t);
r=(u*t)+(0.5*(FRICTIONAL_DECELERATION)*t*t);
}
unitVector(frontdirx,frontdiry,frontdirz);
x*=r;
y*=r;
z*=r;
carPositionx+=x;
carPositiony+=y;
carPositionz+=z;
anglex+=theta*(180.0/M_PI);
t-=INCREMENT;
u=v;
omega0=omega;
cameray=carPositiony+6.0f;
cameraz=carPositionz+13.0f;
referencez=carPositionz;
}
label2:
if(keyStates['w'])
{
t+=INCREMENT;
if(v>=MAXIMUM_VELOCITY_FOR_SATURATION)
{
r=v*t;
theta=omega*t;
unitVector(frontdirx,frontdiry,frontdirz);
x*=r;
y*=r;
z*=r;
carPositionx+=x;
carPositiony+=y;
carPositionz+=z;
anglex+=theta*(180.0/M_PI);
t-=INCREMENT;
cameray=carPositiony+6.0f;
cameraz=carPositionz+13.0f;
referencez=carPositionz;
}
else
{
if(v<0)
{
v=u+(ACCELERATION+FRICTIONAL_DECELERATION)*t;
omega=omega0+(ROTATIONAL_ACCELERATION+ROTATIONAL_DECCELERATION)*t;
theta=(omega0*t)+(0.5*(ROTATIONAL_ACCELERATION+ROTATIONAL_DECCELERATION)*t*t);
r=(u*t)+(0.5*(ACCELERATION+FRICTIONAL_DECELERATION)*t*t);
}
else
{
v=u+(ACCELERATION-FRICTIONAL_DECELERATION)*t;
omega=omega0+(ROTATIONAL_ACCELERATION-ROTATIONAL_DECCELERATION)*t;
theta=(omega0*t)+(0.5*(ROTATIONAL_ACCELERATION-ROTATIONAL_DECCELERATION)*t*t);
r=(u*t)+(0.5*(ACCELERATION-FRICTIONAL_DECELERATION)*t*t);
}
unitVector(frontdirx,frontdiry,frontdirz);
x*=r;
y*=r;
z*=r;
carPositionx+=x;
carPositiony+=y;
carPositionz+=z;
anglex+=theta*(180.0/M_PI);
t-=INCREMENT;
u=v;
omega0=omega;
cameray=carPositiony+6.0f;
cameraz=carPositionz+13.0f;
//fprintf(stdout,"\ncameraz=%f\ncarPositionz=%f\n",cameraz,carPositionz);
referencez=carPositionz;
}
}
if(keyStates['s'])
{
t+=INCREMENT;
if(v<=-MAXIMUM_VELOCITY_FOR_SATURATION)
{
r=v*t;
theta=omega*t;
unitVector(frontdirx,frontdiry,frontdirz);
x*=r;
y*=r;
z*=r;
carPositionx+=x;
carPositiony+=y;
carPositionz+=z;
anglex+=theta*(180.0/M_PI);
t-=INCREMENT;
cameray=carPositiony+6.0f;
cameraz=carPositionz+13.0f;
referencez=carPositionz;
}
else
{
if(v>=0)
{
v=u+(-ACCELERATION-FRICTIONAL_DECELERATION)*t;
omega=omega0+(-ROTATIONAL_ACCELERATION-ROTATIONAL_DECCELERATION)*t;
theta=(omega0*t)+(0.5*(-ROTATIONAL_ACCELERATION-ROTATIONAL_DECCELERATION)*t*t);
r=(u*t)+(0.5*(-ACCELERATION-FRICTIONAL_DECELERATION)*t*t);
}
else
{
v=u+(-ACCELERATION+FRICTIONAL_DECELERATION)*t;
omega=omega0+(-ROTATIONAL_ACCELERATION+ROTATIONAL_DECCELERATION)*t;
theta=(omega0*t)+(0.5*(-ROTATIONAL_ACCELERATION+ROTATIONAL_DECCELERATION)*t*t);
r=(u*t)+(0.5*(-ACCELERATION+FRICTIONAL_DECELERATION)*t*t);
}
unitVector(frontdirx,frontdiry,frontdirz);
x*=r;
y*=r;
z*=r;
carPositionx+=x;
carPositiony+=y;
carPositionz+=z;
anglex+=theta*(180.0/M_PI);
t-=INCREMENT;
u=v;
omega0=omega;
cameray=carPositiony+6.0f;
cameraz=carPositionz+13.0f;
referencez=carPositionz;
}
}
if(keyStates[27])
{
glutHideWindow();
exit(0);
}
if(keySpecialStates[GLUT_KEY_LEFT])
{
if(camerax>=-TRACK_WIDTH/2.0f)
camerax-=0.05;
}
if(keySpecialStates[GLUT_KEY_RIGHT])
{
if(camerax<=TRACK_WIDTH/2.0f)
camerax+=0.05;
}
if(keySpecialStates[GLUT_KEY_UP])
{
unitVector(referencex-camerax,referencey-cameray,referencez-cameraz);
if(mod!=0 && mod>=6)
{
camerax+=DISTANCE*x;
cameray+=DISTANCE*y;
cameraz+=DISTANCE*z;
}
}
if(keySpecialStates[GLUT_KEY_DOWN])
{
unitVector(referencex-camerax,referencey-cameray,referencez-cameraz);
if(mod!=0 && mod<=14)
{
camerax-=DISTANCE*x;
cameray-=DISTANCE*y;
cameraz-=DISTANCE*z;
}
}
}
bool ReachabilityDummyInterface::isReachable(const octomath::Pose6D &pose) const
{
Capability cap;
if (pose.y() > 0.04 && pose.y() < 0.06 && pose.z() > 0.44 && pose.z() < 0.46)
{
if (pose.x() > 0.14 && pose.x() < 0.16)
{
cap = Capability(SPHERE, 0.0, 0.0, 0.0);
}
else if (pose.x() > 0.34 && pose.x() < 0.36)
{
cap = Capability(CONE, 0.0, 90.0, 15.0);
}
else if (pose.x() > 0.54 && pose.x() < 0.56)
{
cap = Capability(CONE, 90.0, 0.0, 45.0);
}
else if (pose.x() > 0.74 && pose.x() < 0.76)
{
cap = Capability(CONE, 90.0, 0.0, 125.0);
}
else if (pose.x() > 0.94 && pose.x() < 0.96)
{
cap = Capability(CONE, 180.0, 120.0, 70.0);
}
else if (pose.x() > 1.14 && pose.x() < 1.16)
{
cap = Capability(CYLINDER_1, 0.0, 0.0, 10.0);
}
else if (pose.x() > 1.34 && pose.x() < 1.36)
{
cap = Capability(CYLINDER_1, 0.0, 0.0, 90.0);
}
else if (pose.x() > 1.54 && pose.x() < 1.56)
{
cap = Capability(CYLINDER_1, 40.0, 50.0, 45.0);
}
else if (pose.x() > 1.74 && pose.x() < 1.76)
{
cap = Capability(CYLINDER_2, 0.0, 0.0, 10.0);
}
else if (pose.x() > 1.94 && pose.x() < 1.96)
{
cap = Capability(CYLINDER_2, 0.0, 0.0, 45.0);
}
else if (pose.x() > 2.14 && pose.x() < 2.16)
{
cap = Capability(CYLINDER_2, 90.0, 90.0, 89.0);
}
else
{
return false;
}
}
else
{
return false;
}
octomath::Vector3 unitVector(1.0, 0.0, 0.0);
octomath::Vector3 rotatedVector = pose.rot().rotate(unitVector);
double phi = atan2(rotatedVector.y(), rotatedVector.x()) * 180.0 / M_PI;
double theta = acos(rotatedVector.z()) * 180.0 / M_PI;
return cap.isDirectionPossible(phi, theta);
}
D3PosDirUpScale &D3PosDirUpScale::setOrientation(const D3DXVECTOR3 &dir, const D3DXVECTOR3 &up) {
m_dir = unitVector(dir);
m_up = unitVector(up);
return updateView();
}
vector2d vector2d::unitVector() const
{
vector2d unitVector(afData[0] / sqrt(afData[0] * afData[0] + afData[1] * afData[1]), afData[1] / sqrt(afData[0] * afData[0] + afData[1] * afData[1]));
return unitVector;
}
/*PUBLIC FUNCTIONS*/
void Projectile::guideTowards(sf::Vector2f position) {
assert(isGuided());
targetDirection = unitVector(position - getWorldPosition());
}
void RubiksCube::Grab(VECTOR touchVector)
{
tvf32[0]+=floattof32(touchVector.X);
tvf32[1]+=floattof32(touchVector.Y);
tvf32[2]=0;
if(f32toint(sqrtf32(mulf32(tvf32[0],tvf32[0])+mulf32(tvf32[1],tvf32[1])+mulf32(tvf32[2],tvf32[2])))>5)
{
VECTOR upv, rightv, rotduv, rotdrv;
int32 uvf32[3];//up vector as f32
int32 rvf32[3];//right vector
int32 magup, magright;
m4x4 grabMatrix;//container for the Position Matrix
vectorFromSide(upv, rightv, clicked[0]);
//printf ("Initial Vector:\n %f, ", tmpv.X);
//printf("%f, ", tmpv.Y);
//printf("%f\n", tmpv.Z);
glGetFixed(GL_GET_MATRIX_POSITION, (int32*)&grabMatrix);
glMatrixMode(GL_MODELVIEW);
//rotate the up vector thru the projection matrix
//and cast it to f32
RotateVector(grabMatrix, upv, rotduv);
uvf32[0]=floattof32(rotduv.X);
uvf32[1]=floattof32(rotduv.Y);
uvf32[2]=floattof32(rotduv.Z);
//rinse and repeat with the right vector
RotateVector(grabMatrix, rightv, rotdrv);
rvf32[0]=floattof32(rotdrv.X);
rvf32[1]=floattof32(rotdrv.Y);
rvf32[2]=floattof32(rotdrv.Z);
if(controlStyle)
{
int32 suvf32[3];
int32 srvf32[3];
suvf32[0]=0;
suvf32[1]=inttof32(1);
suvf32[2]=0;
srvf32[0]=inttof32(1);
srvf32[1]=0;
srvf32[2]=0;
magup=dotf32(uvf32, suvf32);
magright=dotf32(uvf32, srvf32);
if(abs(magup)>abs(magright))
{
for(int i=0; i<3; i++)
{
if(magup>0)
{
rvf32[i]=srvf32[i];
uvf32[i]=suvf32[i];
}
else
{
rvf32[i]=-srvf32[i];
uvf32[i]=-suvf32[i];
}
}
} else {
for(int i=0; i<3; i++)
{
if(magright>0)
{
uvf32[i]=srvf32[i];
rvf32[i]=-suvf32[i];
}
else
{
uvf32[i]=-srvf32[i];
rvf32[i]=suvf32[i];
}
}
}
}
magup=dotf32(uvf32, tvf32);
magright=dotf32(rvf32, tvf32);
if(magup || magright)
{
int32 tmp[2];
if(abs(magup)>abs(magright))
{
tmp[0]=uvf32[0];
tmp[1]=uvf32[1];
unitVector((int32*)tmp);
InitTwist(true, tmp);
}else{
tmp[0]=rvf32[0];
tmp[1]=rvf32[1];
unitVector((int32*)tmp);
InitTwist(false, tmp);
}
Twisting=true;
Grabbing=false;
tvf32[0]=0;
tvf32[1]=0;
}
}
}
void polygonClipping(face_info* face){
int i=0;
vertex* verts=face->vertex_set;
int count=face->number_of_vertices;
std::vector<vertex*> CvTable;
std::vector<vertex*> CvTabletemp;
for(i=0;i<count;i++){
vertex* point=verts+i;
CvTable.push_back(point);
}
for(i=0;i<clipping_plane_eq.size();i++)
{
float* eq_plane=clipping_plane_eq.at(i);
//printf("equation of plane %d %f %f %f %f \n",i,eq_plane[0],eq_plane[1],eq_plane[2],eq_plane[3]);
int j;
for(j=0;j<CvTable.size();j++)
{
vertex* point1=CvTable.at(j);
vertex* point2=CvTable.at((j+1)%CvTable.size());
vertex* unitvect=unitVector(point1,point2);
Ray* ray=(Ray*)malloc(sizeof(Ray));
ray->direction=unitvect;
ray->startPoint=point1;
//printf("point1=%f %f %f\n",point1->x_pos,point1->y_pos,point1->z_pos);
//printf("point2=%f %f %f\n",point2->x_pos,point2->y_pos,point2->z_pos);
int k;
if(isInsidePlane(eq_plane,point1)) //if v1 is inside
{
if(isInsidePlane(eq_plane,point2)) //if v2 is inside
{
//printf("1\n");
CvTabletemp.push_back(point2);
}
else //if v2 is outside
{
//find intersection point and put in cvtable
vertex* temp=findIntersection(eq_plane,ray);
if(temp!=NULL)
CvTabletemp.push_back(temp);
}
}
else //if v1 is outside
{
if(isInsidePlane(eq_plane,point2)) //if v2 is inside
{
//find intersection point and put in cvtable
vertex* temp=findIntersection(eq_plane,ray);
if(temp!=NULL)
CvTabletemp.push_back(temp);
CvTabletemp.push_back(point2);
}
}
}
CvTable.clear();
int k;
for(k=0;k<CvTabletemp.size();k++){
CvTable.push_back(CvTabletemp.at(k));
}
CvTabletemp.clear();
}
vertex* newverts= (vertex*)malloc(CvTable.size()*sizeof(vertex));
for(i=0;i<CvTable.size();i++){
newverts[i]=CvTable.at(i)[0];
}
face->vertex_set=newverts;
face->number_of_vertices=CvTable.size();
}
