void UprightFrame::unwrapYaw(UprightFrame* a, int N) {
// Use the first point to establish the wrapping convention
for (int i = 1; i < N; ++i) {
const float prev = a[i - 1].yaw;
float& cur = a[i].yaw;
// No two angles should be more than pi (i.e., 180-degrees) apart.
if (abs(cur - prev) > G3D::pi()) {
// These angles must have wrapped at zero, causing them
// to be interpolated the long way.
// Find canonical [0, 2pi] versions of these numbers
float p = wrap(prev, twoPi());
float c = wrap(cur, twoPi());
// Find the difference -pi < diff < pi between the current and previous values
float diff = c - p;
if (diff < -G3D::pi()) {
diff += twoPi();
} else if (diff > G3D::pi()) {
diff -= twoPi();
}
// Offset the current from the previous by the difference
// between them.
cur = prev + diff;
}
}
}
float Cylinder::area() const {
return
// Sides
(twoPi() * mRadius) * height() +
// Caps
twoPi() * square(mRadius);
}
void Cylinder::getRandomSurfacePoint(Vector3& p, Vector3& N) const {
float h = height();
float r = radius();
// Create a random point on a standard cylinder and then rotate to the global frame.
// Relative areas (factor of 2PI already taken out)
float capRelArea = square(r) / 2.0f;
float sideRelArea = r * h;
float r1 = uniformRandom(0, capRelArea * 2 + sideRelArea);
if (r1 < capRelArea * 2) {
// Select a point uniformly at random on a disk
// @cite http://mathworld.wolfram.com/DiskPointPicking.html
float a = uniformRandom(0, (float)twoPi());
float r2 = sqrt(uniformRandom(0, 1)) * r;
p.x = cos(a) * r2;
p.z = sin(a) * r2;
N.x = 0;
N.z = 0;
if (r1 < capRelArea) {
// Top
p.y = h / 2.0f;
N.y = 1;
} else {
// Bottom
p.y = -h / 2.0f;
N.y = -1;
}
} else {
// Side
float a = uniformRandom(0, (float)twoPi());
N.x = cos(a);
N.y = 0;
N.z = sin(a);
p.x = N.x * r;
p.z = N.y * r;
p.y = uniformRandom(-h / 2.0f, h / 2.0f);
}
// Transform to world space
CoordinateFrame cframe;
getReferenceFrame(cframe);
p = cframe.pointToWorldSpace(p);
N = cframe.normalToWorldSpace(N);
}
float Capsule::area() const {
return
// Sphere area
pow(_radius, 2) * 4 * pi() +
// Cylinder area
twoPi() * _radius * (p1 - p2).magnitude();
}
void Quat::toAxisAngleRotation(
Vector3& axis,
double& angle) const {
// Decompose the quaternion into an angle and an axis.
axis = Vector3(x, y, z);
angle = 2 * acos(w);
float len = sqrt(1.0f - w * w);
if (fuzzyGt(abs(len), 0.0f)) {
axis /= len;
}
// Reduce the range of the angle.
if (angle < 0) {
angle = -angle;
axis = -axis;
}
while (angle > twoPi()) {
angle -= twoPi();
}
if (abs(angle) > pi()) {
angle -= twoPi();
}
// Make the angle positive.
if (angle < 0.0f) {
angle = -angle;
axis = -axis;
}
}
void FunctionApproximatorIRFRLS::train(const MatrixXd& inputs, const MatrixXd& targets)
{
if (isTrained())
{
cerr << "WARNING: You may not call FunctionApproximatorIRFRLS::train more than once. Doing nothing." << endl;
cerr << " (if you really want to retrain, call reTrain function instead)" << endl;
return;
}
assert(inputs.rows() == targets.rows()); // Must have same number of examples
assert(inputs.cols()==getExpectedInputDim());
const MetaParametersIRFRLS* meta_parameters_irfrls =
static_cast<const MetaParametersIRFRLS*>(getMetaParameters());
int nb_cos = meta_parameters_irfrls->number_of_basis_functions_;
// Init random generator.
boost::mt19937 rng(getpid() + time(0));
// Draw periodes
boost::normal_distribution<> twoGamma(0, sqrt(2 * meta_parameters_irfrls->gamma_));
boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > genPeriods(rng, twoGamma);
MatrixXd cosines_periodes(nb_cos, inputs.cols());
for (int r = 0; r < nb_cos; r++)
for (int c = 0; c < inputs.cols(); c++)
cosines_periodes(r, c) = genPeriods();
// Draw phase
boost::uniform_real<> twoPi(0, 2 * boost::math::constants::pi<double>());
boost::variate_generator<boost::mt19937&, boost::uniform_real<> > genPhases(rng, twoPi);
VectorXd cosines_phase(nb_cos);
for (int r = 0; r < nb_cos; r++)
cosines_phase(r) = genPhases();
MatrixXd proj_inputs;
proj(inputs, cosines_periodes, cosines_phase, proj_inputs);
// Compute linear model analatically
double lambda = meta_parameters_irfrls->lambda_;
MatrixXd toInverse = lambda * MatrixXd::Identity(nb_cos, nb_cos) + proj_inputs.transpose() * proj_inputs;
VectorXd linear_model = toInverse.inverse() *
(proj_inputs.transpose() * targets);
setModelParameters(new ModelParametersIRFRLS(linear_model, cosines_periodes, cosines_phase));
}
Vector3 Cylinder::randomInteriorPoint() const {
float h = height();
float r = radius();
// Create a random point in a standard cylinder and then rotate to the global frame.
// Select a point uniformly at random on a disk
// @cite http://mathworld.wolfram.com/DiskPointPicking.html
float a = uniformRandom(0, (float)twoPi());
float r2 = sqrt(uniformRandom(0, 1)) * r;
Vector3 p( cos(a) * r2,
uniformRandom(-h / 2.0f, h / 2.0f),
sin(a) * r2);
// Transform to world space
CoordinateFrame cframe;
getReferenceFrame(cframe);
return cframe.pointToWorldSpace(p);
}
void Capsule::getRandomSurfacePoint(Vector3& p, Vector3& N) const {
float h = height();
float r = radius();
// Create a random point on a standard capsule and then rotate to the global frame.
// Relative areas
float capRelArea = sqrt(r) / 2.0f;
float sideRelArea = r * h;
float r1 = uniformRandom(0, capRelArea * 2 + sideRelArea);
if (r1 < capRelArea * 2) {
// Select a point uniformly at random on a sphere
N = Sphere(Vector3::zero(), 1).randomSurfacePoint();
p = N * r;
p.y += sign(p.y) * h / 2.0f;
} else {
// Side
float a = uniformRandom(0, (float)twoPi());
N.x = cos(a);
N.y = 0;
N.z = sin(a);
p.x = N.x * r;
p.z = N.y * r;
p.y = uniformRandom(-h / 2.0f, h / 2.0f);
}
// Transform to world space
CoordinateFrame cframe;
getReferenceFrame(cframe);
p = cframe.pointToWorldSpace(p);
N = cframe.normalToWorldSpace(N);
}
Vector3 Capsule::randomInteriorPoint() const {
float h = height();
float r = radius();
// Create a random point in a standard capsule and then rotate to the global frame.
Vector3 p;
float hemiVolume = pi() * (r*r*r) * 4 / 6.0;
float cylVolume = pi() * square(r) * h;
float r1 = uniformRandom(0, 2.0 * hemiVolume + cylVolume);
if (r1 < 2.0 * hemiVolume) {
p = Sphere(Vector3::zero(), r).randomInteriorPoint();
p.y += sign(p.y) * h / 2.0f;
} else {
// Select a point uniformly at random on a disk
float a = uniformRandom(0, (float)twoPi());
float r2 = sqrt(uniformRandom(0, 1)) * r;
p = Vector3(cos(a) * r2,
uniformRandom(-h / 2.0f, h / 2.0f),
sin(a) * r2);
}
// Transform to world space
CoordinateFrame cframe;
getReferenceFrame(cframe);
return cframe.pointToWorldSpace(p);
}
virtual CFrame computeFrame(SimTime time) const override {
const float angle = float(twoPi() * time) / m_period;
return CFrame::fromXYZYPRRadians(sin(angle) * m_radius, 0, cos(angle) * m_radius, angle);
}
ThirdPersonManipulator::ThirdPersonManipulator() :
m_axisScale(1),
m_dragging(false),
m_dragKey(GKey::LEFT_MOUSE),
m_doubleAxisDrag(false),
m_overAxis(NO_AXIS),
m_maxAxisDistance2D(15),
m_maxRotationDistance2D(10),
m_rotationEnabled(true),
m_translationEnabled(true),
m_enabled(true) {
for (int i = 0; i < NUM_GEOMS; ++i) {
m_usingAxis[i] = false;
}
// Size of the 2-axis control widget
const float hi = 0.80f, lo = 0.60f;
// Single axis (cut a hole out of the center for grabbing all)
const float cut = 0.20f;
Array<Vector3> vertex;
// Create the translation axes. They will be rendered separately.
vertex.clear();
vertex.append(Vector3(cut, 0, 0), Vector3(1.1f, 0, 0));
m_geomArray[X].line3D.append(_internal::PolyLine(vertex));
m_geomArray[X].visible = false;
vertex.clear();
vertex.append(Vector3(0, cut, 0), Vector3(0, 1.1f, 0));
m_geomArray[Y].line3D.append(_internal::PolyLine(vertex));
m_geomArray[Y].visible = false;
vertex.clear();
vertex.append(Vector3(0, 0, cut), Vector3(0, 0, 1.1f));
m_geomArray[Z].line3D.append(_internal::PolyLine(vertex));
m_geomArray[Z].visible = false;
// Create the translation squares that lie inbetween the axes
if (m_doubleAxisDrag) {
vertex.clear();
vertex.append(Vector3(lo, hi, 0), Vector3(lo, lo, 0), Vector3(hi, lo, 0), Vector3(hi, hi, 0));
m_geomArray[XY].poly3D.append(ConvexPolygon(vertex));
vertex.clear();
vertex.append(Vector3(0, hi, hi), Vector3(0, lo, hi), Vector3(0, lo, lo), Vector3(0, hi, lo));
m_geomArray[YZ].poly3D.append(ConvexPolygon(vertex));
vertex.clear();
vertex.append(Vector3(lo, 0, lo), Vector3(lo, 0, hi), Vector3(hi, 0, hi), Vector3(hi, 0, lo));
m_geomArray[ZX].poly3D.append(ConvexPolygon(vertex));
}
// Create the rotation circles
const int ROT_SEGMENTS = 40;
const float ROT_RADIUS = 0.65f;
vertex.resize(ROT_SEGMENTS + 1);
for (int v = 0; v <= ROT_SEGMENTS; ++v) {
float a = twoPi() * (float)v / ROT_SEGMENTS;
vertex[v].x = 0;
vertex[v].y = cos(a) * ROT_RADIUS;
vertex[v].z = sin(a) * ROT_RADIUS;
}
m_geomArray[RX].line3D.append(_internal::PolyLine(vertex));
for (int v = 0; v <= ROT_SEGMENTS; ++v) {
vertex[v] = vertex[v].zxy();
}
m_geomArray[RY].line3D.append(_internal::PolyLine(vertex));
for (int v = 0; v <= ROT_SEGMENTS; ++v) {
vertex[v] = vertex[v].zxy();
}
m_geomArray[RZ].line3D.append(_internal::PolyLine(vertex));
m_posedModel = new TPMPosedModel(this);
}
/** If given an IFS filename, loads it, otherwise generates
a gear with high vertex coherence.
Only the constructor uses G3D. */
Model(const std::string& filename) {
tex = Texture::fromFile("tiny.jpg");
textureID = tex->openGLID();
if (filename != "") {
Surface::Ref m = IFSModel::fromFile(filename)->pose();
// Copy fields out into arrays
{
const Array<int>& index = m->triangleIndices();
int N = index.size();
cpuIndex.resize(N);
System::memcpy(&cpuIndex[0], index.getCArray(), sizeof(int) * N);
}
{
const MeshAlg::Geometry& g = m->objectSpaceGeometry();
int N = g.vertexArray.size();
cpuVertex.resize(N);
cpuNormal.resize(N);
cpuColor.resize(N);
cpuTexCoord.resize(N);
System::memcpy(&cpuVertex[0], g.vertexArray.getCArray(), N * sizeof(Vec3));
System::memcpy(&cpuNormal[0], g.normalArray.getCArray(), N * sizeof(Vec3));
for (int i = 0; i < N; ++i) {
// Copy the normals over the colors, too
cpuColor[i].x = g.normalArray[i].x * 0.5 + 0.5;
cpuColor[i].y = g.normalArray[i].y * 0.5 + 0.5;
cpuColor[i].z = g.normalArray[i].z * 0.5 + 0.5;
cpuColor[i].w = 1.0f;
// Cylindrical projection to get tex coords
Vector3 dir = g.vertexArray[i].direction();
cpuTexCoord[i].x = (atan2(dir.x, dir.z) / twoPi() + 0.5) * 5;
cpuTexCoord[i].y = (0.5 - dir.y * 0.5) * 5;
}
}
} else {
// Generate gear
// Vertices per side
const int sq = 187;
// Number of indices
const int N = (sq - 1) * (sq - 1) * 3 * 2;
// Number of vertices
const int V = sq * sq;
// Make a grid of triangles
cpuIndex.resize(N);
{
int k = 0;
for (int i = 0; i < sq - 1; ++i) {
for (int j = 0; j < sq - 1; ++j) {
debugAssert(k < N - 5);
// Bottom triangle
cpuIndex[k + 0] = i + j * sq;
cpuIndex[k + 1] = i + (j + 1) * sq;
cpuIndex[k + 2] = (i + 1) + (j + 1) * sq;
// Top triangle
cpuIndex[k + 3] = i + j * sq;
cpuIndex[k + 4] = (i + 1) + (j + 1) * sq;
cpuIndex[k + 5] = (i + 1) + j * sq;
k += 6;
}
}
}
// Create data
cpuVertex.resize(V);
cpuNormal.resize(V);
cpuTexCoord.resize(V);
cpuColor.resize(V);
// Map V indices to a sq x sq grid
for (int i = 0; i < sq; ++i) {
for (int j = 0; j < sq; ++j) {
int v = (i + j * sq);
float x = (i / (float)sq - 0.5) * 2;
float y = 0.5 - j / (float)sq;
float a = x * 2 * 3.1415927;
float r = ceil(cos(a * 10)) * 0.05 + 0.3;
cpuVertex[v].x = -cos(a) * r;
cpuVertex[v].y = y;
cpuVertex[v].z = sin(a) * r;
// Scale the normal
float s = 1.0 / sqrt(0.0001 + square(cpuVertex[v].x) + square(cpuVertex[v].y) + square(cpuVertex[v].z));
cpuNormal[v].y = cpuVertex[v].x * s;
cpuNormal[v].x = cpuVertex[v].y * s;
cpuNormal[v].z = cpuVertex[v].z * s;
cpuColor[v].x = r + 0.7;
cpuColor[v].y = 0.5;
cpuColor[v].z = 1.0 - r;
cpuColor[v].w = 1.0f;
cpuTexCoord[v].x = i / (float)sq;
cpuTexCoord[v].y = j / (float)sq;
}
}
} // if
cpuIndex16.resize(cpuIndex.size());
for (int i = 0; i < (int)cpuIndex.size(); ++i) {
cpuIndex16[i] = (G3D::uint16)cpuIndex[i];
}
}
