void CQTOpenGLEPuck::RenderLED() {
/* Side surface */
CVector2 cVertex(BODY_RADIUS, 0.0f);
CRadians cAngle(CRadians::TWO_PI / m_unVertices);
CVector2 cNormal(1.0f, 0.0f);
glBegin(GL_QUAD_STRIP);
for(GLuint i = 0; i <= m_unVertices / 8; i++) {
glNormal3f(cNormal.GetX(), cNormal.GetY(), 0.0f);
glVertex3f(cVertex.GetX(), cVertex.GetY(), LED_ELEVATION + LED_HEIGHT);
glVertex3f(cVertex.GetX(), cVertex.GetY(), LED_ELEVATION);
cVertex.Rotate(cAngle);
cNormal.Rotate(cAngle);
}
glEnd();
/* Top surface */
cVertex.Set(BODY_RADIUS, 0.0f);
CVector2 cVertex2(LED_UPPER_RING_INNER_RADIUS, 0.0f);
glBegin(GL_QUAD_STRIP);
glNormal3f(0.0f, 0.0f, 1.0f);
for(GLuint i = 0; i <= m_unVertices / 8; i++) {
glVertex3f(cVertex2.GetX(), cVertex2.GetY(), BODY_ELEVATION + BODY_HEIGHT + LED_HEIGHT);
glVertex3f(cVertex.GetX(), cVertex.GetY(), BODY_ELEVATION + BODY_HEIGHT + LED_HEIGHT);
cVertex.Rotate(cAngle);
cVertex2.Rotate(cAngle);
}
glEnd();
}
void experienceNet::normalPDF_sse(float* result, const float* _partitions, float _mean, float _stdDev)
{
/*
CODE ADAPTED FROM boost/math/normal.hpp
RealType exponent = x - mean;
exponent *= -exponent;
exponent /= 2 * sd * sd;
result = exp(exponent);
result /= sd * sqrt(2 * constants::pi<RealType>());
return result;
*/
const __m128& partitions = *(__m128*)_partitions;
__m128 exponent, tmp, mean, sd;
/* CODE ADAPTED FROM http://fastcpp.blogspot.com/2011/03/changing-sign-of-float-values-using-sse.html */
static const __m128 signmask = _mm_castsi128_ps(_mm_set1_epi32(0x80000000));
static const __m128 twos = _mm_set_ps1(2.0f);
static const __m128 sqrt_pi_2_s = _mm_set_ps1(sqrt(2.0 * M_PI));
// store mean and sd:
mean = _mm_load_ps1(&_mean);
sd = _mm_load_ps1(&_stdDev);
// exponent = x - mean
exponent = _mm_sub_ps(partitions, mean);
// exponent *= -exponent;
tmp = _mm_xor_ps(exponent, signmask);
exponent = _mm_mul_ps(exponent, tmp);
// exponent /= 2 * sd * sd;
tmp = _mm_mul_ps(sd, sd);
tmp = _mm_mul_ps(tmp, twos);
exponent = _mm_div_ps(exponent, tmp);
// exponent = exp(exponent);
exponent = _mm_exp_ps(exponent);
// exponent /= sd * sqrt(2 * pi)
tmp = _mm_mul_ps(sd, sqrt_pi_2_s);
tmp = _mm_div_ps(exponent, tmp);
#ifndef NDEBUG
const float* _result = (float*)&tmp;
boost::math::normal_distribution<float> cNormal(_mean, _stdDev);
assert(fastabs(_result[0] - boost::math::pdf(cNormal, _partitions[0])) < 0.001f);
assert(fastabs(_result[1] - boost::math::pdf(cNormal, _partitions[1])) < 0.001f);
assert(fastabs(_result[2] - boost::math::pdf(cNormal, _partitions[2])) < 0.001f);
assert(fastabs(_result[3] - boost::math::pdf(cNormal, _partitions[3])) < 0.001f);
#endif
// return result:
_mm_store_ps(result, tmp);
};
void CQTOpenGLEPuck::RenderBody() {
/* Set material */
SetGreenPlasticMaterial();
CVector2 cVertex(BODY_RADIUS, 0.0f);
CRadians cAngle(-CRadians::TWO_PI / m_unVertices);
/* Bottom part */
glBegin(GL_POLYGON);
glNormal3f(0.0f, 0.0f, -1.0f);
for(GLuint i = 0; i <= m_unVertices; i++) {
glVertex3f(cVertex.GetX(), cVertex.GetY(), BODY_ELEVATION);
cVertex.Rotate(cAngle);
}
glEnd();
/* Side surface */
cAngle = -cAngle;
CVector2 cNormal(1.0f, 0.0f);
cVertex.Set(BODY_RADIUS, 0.0f);
glBegin(GL_QUAD_STRIP);
for(GLuint i = 0; i <= m_unVertices; i++) {
glNormal3f(cNormal.GetX(), cNormal.GetY(), 0.0f);
glVertex3f(cVertex.GetX(), cVertex.GetY(), BODY_ELEVATION + BODY_HEIGHT);
glVertex3f(cVertex.GetX(), cVertex.GetY(), BODY_ELEVATION);
cVertex.Rotate(cAngle);
cNormal.Rotate(cAngle);
}
glEnd();
/* Top part */
glBegin(GL_POLYGON);
cVertex.Set(LED_UPPER_RING_INNER_RADIUS, 0.0f);
glNormal3f(0.0f, 0.0f, 1.0f);
for(GLuint i = 0; i <= m_unVertices; i++) {
glVertex3f(cVertex.GetX(), cVertex.GetY(), BODY_ELEVATION + BODY_HEIGHT + LED_HEIGHT);
cVertex.Rotate(cAngle);
}
glEnd();
/* Triangle to set the direction */
SetLEDMaterial(1.0f, 1.0f, 0.0f);
glBegin(GL_TRIANGLES);
glVertex3f( BODY_RADIUS * 0.7, 0.0f, BODY_ELEVATION + BODY_HEIGHT + LED_HEIGHT + 0.001f);
glVertex3f(-BODY_RADIUS * 0.7, BODY_RADIUS * 0.3, BODY_ELEVATION + BODY_HEIGHT + LED_HEIGHT + 0.001f);
glVertex3f(-BODY_RADIUS * 0.7, -BODY_RADIUS * 0.3, BODY_ELEVATION + BODY_HEIGHT + LED_HEIGHT + 0.001f);
glEnd();
}
void CQTOpenGLEPuck::RenderWheel() {
/* Set material */
SetRedPlasticMaterial();
/* Right side */
CVector2 cVertex(WHEEL_RADIUS, 0.0f);
CRadians cAngle(CRadians::TWO_PI / m_unVertices);
CVector3 cNormal(-1.0f, -1.0f, 0.0f);
cNormal.Normalize();
glBegin(GL_POLYGON);
for(GLuint i = 0; i <= m_unVertices; i++) {
glNormal3f(cNormal.GetX(), cNormal.GetY(), cNormal.GetZ());
glVertex3f(cVertex.GetX(), -HALF_WHEEL_WIDTH, WHEEL_RADIUS + cVertex.GetY());
cVertex.Rotate(cAngle);
cNormal.RotateY(cAngle);
}
glEnd();
/* Left side */
cVertex.Set(WHEEL_RADIUS, 0.0f);
cNormal.Set(-1.0f, 1.0f, 0.0f);
cNormal.Normalize();
cAngle = -cAngle;
glBegin(GL_POLYGON);
for(GLuint i = 0; i <= m_unVertices; i++) {
glNormal3f(cNormal.GetX(), cNormal.GetY(), cNormal.GetZ());
glVertex3f(cVertex.GetX(), HALF_WHEEL_WIDTH, WHEEL_RADIUS + cVertex.GetY());
cVertex.Rotate(cAngle);
cNormal.RotateY(cAngle);
}
glEnd();
/* Tire */
cNormal.Set(1.0f, 0.0f, 0.0f);
cVertex.Set(WHEEL_RADIUS, 0.0f);
cAngle = -cAngle;
glBegin(GL_QUAD_STRIP);
for(GLuint i = 0; i <= m_unVertices; i++) {
glNormal3f(cNormal.GetX(), cNormal.GetY(), cNormal.GetZ());
glVertex3f(cVertex.GetX(), -HALF_WHEEL_WIDTH, WHEEL_RADIUS + cVertex.GetY());
glVertex3f(cVertex.GetX(), HALF_WHEEL_WIDTH, WHEEL_RADIUS + cVertex.GetY());
cVertex.Rotate(cAngle);
cNormal.RotateY(cAngle);
}
glEnd();
}
bool CCylinder::Intersects(Real& f_t_on_ray,
const CRay3& c_ray) {
/*
* This algorithm was adapted from
* http://www.realtimerendering.com/resources/GraphicsGems/gemsiv/ray_cyl.c
*/
/* Vector from cylinder base to ray start */
CVector3 cCylBase2RayStart(c_ray.GetStart());
cCylBase2RayStart -= m_cBasePos;
/* Ray direction and length */
CVector3 cRayDir;
c_ray.GetDirection(cRayDir);
Real fRayLen = c_ray.GetLength();
/* Vector normal to cylinder axis and ray direction */
CVector3 cNormal(cRayDir);
cNormal.CrossProduct(m_cAxis);
Real fNormalLen = cNormal.Length();
/* Are cylinder axis and ray parallel? */
if(fNormalLen > 0) {
/* No, they aren't parallel */
/* Make normal have length 1 */
cNormal /= fNormalLen;
/* Calculate shortest distance between axis and ray
* by projecting cCylBase2RayStart onto cNormal */
Real fDist = Abs(cCylBase2RayStart.DotProduct(cNormal));
/* Is fDist smaller than the cylinder radius? */
if(fDist > m_fRadius) {
/* No, it's not, so there can't be any intersection */
return false;
}
/* If we get here, it's because the ray intersects the infinite cylinder */
/* Create a buffer for the 4 potential intersection points
(two on the sides, two on the bases) */
Real fPotentialT[4];
/* First, calculate the intersection points with the sides */
/* Calculate the midpoint between the two intersection points */
CVector3 cVec(cCylBase2RayStart);
cVec.CrossProduct(m_cAxis);
Real fMidPointDist = -cVec.DotProduct(cNormal) / fNormalLen;
/* Calculate the distance between the midpoint and the potential t's */
cVec = cNormal;
cVec.CrossProduct(m_cAxis);
cVec.Normalize();
Real fDeltaToMidPoint = Abs(Sqrt(Square(m_fRadius) - Square(fDist)) / cRayDir.DotProduct(cVec));
/* Calculate the potential t's on the infinite surface */
fPotentialT[0] = (fMidPointDist - fDeltaToMidPoint) / fRayLen;
fPotentialT[1] = (fMidPointDist + fDeltaToMidPoint) / fRayLen;
/* Make sure these t's correspond to points within the cylinder bases */
CVector3 cPoint;
c_ray.GetPoint(cPoint, fPotentialT[0]);
if((cPoint - m_cBasePos).DotProduct(m_cAxis) < 0 ||
(cPoint - (m_cBasePos + m_fHeight * m_cAxis)).DotProduct(m_cAxis) > 0) {
fPotentialT[0] = -1;
}
c_ray.GetPoint(cPoint, fPotentialT[1]);
if((cPoint - m_cBasePos).DotProduct(m_cAxis) < 0 ||
(cPoint - (m_cBasePos + m_fHeight * m_cAxis)).DotProduct(m_cAxis) > 0) {
fPotentialT[1] = -1;
}
/* Check whether the ray is contained within the cylinder bases */
Real fDenominator = cRayDir.DotProduct(m_cAxis);
/* Is ray parallel to plane? */
if(Abs(fDenominator) > 1e-6) {
/* No, it's not parallel */
fDenominator *= fRayLen;
/* Bottom base */
fPotentialT[2] =
(m_cBasePos - c_ray.GetStart()).DotProduct(m_cAxis) / fDenominator;
/* Top base */
fPotentialT[3] =
(m_cBasePos + m_fHeight * m_cAxis - c_ray.GetStart()).DotProduct(m_cAxis) / fDenominator;
/* Make sure these t's are within the cylinder surface */
c_ray.GetPoint(cPoint, fPotentialT[2]);
CVector3 cDiff = cPoint - m_cBasePos;
if((cDiff - cDiff.DotProduct(m_cAxis) * m_cAxis).SquareLength() > Square(m_fRadius))
fPotentialT[2] = -1;
c_ray.GetPoint(cPoint, fPotentialT[3]);
cDiff = cPoint - m_cBasePos;
if((cDiff - cDiff.DotProduct(m_cAxis) * m_cAxis).SquareLength() > Square(m_fRadius))
fPotentialT[3] = -1;
}
else {
/* Yes, it's parallel - discard the intersections */
fPotentialT[2] = -1.0;
fPotentialT[3] = -1.0;
}
/* Go through all the potential t's and get the best */
f_t_on_ray = 2.0;
for(UInt32 i = 0; i < 4; ++i) {
if(fPotentialT[i] > 0.0f) {
f_t_on_ray = Min(f_t_on_ray, fPotentialT[i]);
}
}
/* Return true only if the intersection point is within the ray limits */
return (f_t_on_ray < 1.0f);
}
else {
/* Yes, ray and axis are parallel */
/* Projection of cCylBase2RayStart onto the axis */
Real fProj = cCylBase2RayStart.DotProduct(m_cAxis);
/* Radial vector */
CVector3 cRadial(cCylBase2RayStart);
cRadial -= fProj * m_cAxis;
Real fDist = cRadial.Length();
/* Is ray within the cylinder radius? */
if(fDist > m_fRadius) {
/* No, it's not */
return false;
}
/* If we get here, it's because the ray might intersect the cylinder bases */
Real fDenominator = cRayDir.DotProduct(m_cAxis) * fRayLen;
/* Create a buffer for the 2 potential intersection points */
Real fPotentialT[2];
/* Bottom base */
fPotentialT[0] =
(m_cBasePos-c_ray.GetStart()).DotProduct(m_cAxis) / fDenominator;
/* Top base */
fPotentialT[1] =
(m_cBasePos + m_fHeight * m_cAxis - c_ray.GetStart()).DotProduct(m_cAxis) / fDenominator;
/* Make sure these t's are within the cylinder surface */
CVector3 cPoint;
c_ray.GetPoint(cPoint, fPotentialT[0]);
CVector3 cDiff = cPoint - m_cBasePos;
if((cDiff - cDiff.DotProduct(m_cAxis) * m_cAxis).SquareLength() > Square(m_fRadius))
fPotentialT[0] = -1;
c_ray.GetPoint(cPoint, fPotentialT[1]);
cDiff = cPoint - m_cBasePos;
if((cDiff - cDiff.DotProduct(m_cAxis) * m_cAxis).SquareLength() > Square(m_fRadius))
fPotentialT[1] = -1;
/* Go through all the potential t's and get the best */
f_t_on_ray = 2.0;
for(UInt32 i = 0; i < 2; ++i) {
if(fPotentialT[i] > 0.0f) {
f_t_on_ray = Min(f_t_on_ray, fPotentialT[i]);
}
}
/* Return true only if the intersection point is within the ray limits */
return (f_t_on_ray < 1.0f);
}
}
