void SphericalDeformation::setScreenUniforms(ptr<SceneNode> context, ptr<TerrainQuad> q, ptr<Program> prog) const
{
vec3d p0 = vec3d(q->ox, q->oy, R);
vec3d p1 = vec3d(q->ox + q->l, q->oy, R);
vec3d p2 = vec3d(q->ox, q->oy + q->l, R);
vec3d p3 = vec3d(q->ox + q->l, q->oy + q->l, R);
vec3d pc = (p0 + p3) * 0.5;
double l0, l1, l2, l3;
vec3d v0 = p0.normalize(&l0);
vec3d v1 = p1.normalize(&l1);
vec3d v2 = p2.normalize(&l2);
vec3d v3 = p3.normalize(&l3);
if (screenQuadCornersU != NULL) {
mat4d deformedCorners = mat4d(
v0.x * R, v1.x * R, v2.x * R, v3.x * R,
v0.y * R, v1.y * R, v2.y * R, v3.y * R,
v0.z * R, v1.z * R, v2.z * R, v3.z * R,
1.0, 1.0, 1.0, 1.0);
screenQuadCornersU->setMatrix((localToScreen * deformedCorners).cast<float>());
}
if (screenQuadVerticalsU != NULL) {
mat4d deformedVerticals = mat4d(
v0.x, v1.x, v2.x, v3.x,
v0.y, v1.y, v2.y, v3.y,
v0.z, v1.z, v2.z, v3.z,
0.0, 0.0, 0.0, 0.0);
screenQuadVerticalsU->setMatrix((localToScreen * deformedVerticals).cast<float>());
}
if (screenQuadCornerNormsU != NULL) {
screenQuadCornerNormsU->set(vec4d(l0, l1, l2, l3).cast<float>());
}
if (tangentFrameToWorldU != NULL) {
vec3d uz = pc.normalize();
vec3d ux = vec3d::UNIT_Y.crossProduct(uz).normalize();
vec3d uy = uz.crossProduct(ux);
mat4d ltow = context->getLocalToWorld();
mat3d tangentFrameToWorld = mat3d(
ltow[0][0], ltow[0][1], ltow[0][2],
ltow[1][0], ltow[1][1], ltow[1][2],
ltow[2][0], ltow[2][1], ltow[2][2]) *
mat3d(
ux.x, uy.x, uz.x,
ux.y, uy.y, uz.y,
ux.z, uy.z, uz.z);
tangentFrameToWorldU->setMatrix(tangentFrameToWorld.cast<float>());
}
}
mat3d calcDirectRotation(const vec3d &tip, const vec3d &target)
{
double lenSqrTip = dot(tip, tip);
if (lenSqrTip < 0.001) return mat3d(1.0);
double lenSqrTarget = dot(target, target);
if (lenSqrTarget < 0.001) return mat3d(1.0);
vec3d a = tip * vmath::rsqrt(lenSqrTip);
vec3d b = target * vmath::rsqrt(lenSqrTarget);
// sanity check
assert(abs(dot(a,a) - 1.0) < 0.0001);
assert(abs(dot(b,b) - 1.0) < 0.0001);
vec3d axis;
double dotAB = dot(a, b);
double angle;
if (dotAB <= -1.0)
{
// angle is 180 degrees; any axis will do...
angle = M_PI;
if (abs(dot(a, unitX)) < 0.8)
axis = normalize(cross(a, unitX));
else
axis = normalize(cross(a, unitZ));
}
else if (dotAB >= 1.0)
{
// angle is zero; no rotation
return mat3d(1.0);
}
else
{
angle = std::acos(dotAB);
axis = cross(a, b);
}
// sanity check
assert(angle >= -M_PI && angle <= M_PI);
// early-out if the angle is small
if (angle < 0.001)
return mat3d(1.0);
return vmath::rotation_matrix3(angle, axis);
}
void SphericalDeformation::setUniforms(ptr<SceneNode> context, ptr<TerrainNode> n, ptr<Program> prog) const
{
if (lastNodeProg != prog) {
radiusU = prog->getUniform1f("deformation.radius");
localToWorldU = prog->getUniformMatrix3f("deformation.localToWorld");
}
Deformation::setUniforms(context, n, prog);
if (localToWorldU != NULL) {
mat4d ltow = context->getLocalToWorld();
localToWorldU->setMatrix(mat3d(
ltow[0][0], ltow[0][1], ltow[0][2],
ltow[1][0], ltow[1][1], ltow[1][2],
ltow[2][0], ltow[2][1], ltow[2][2]).cast<float>());
}
if (radiusU != NULL) {
radiusU->set(R);
}
}
vec3d IkSolver::updateJointByIk(const Bone &b, const Bone::Connection &joint, const vec3d &target, const vec3d &tip)
{
BoneState &bs = boneStates[b.id];
const vec3d relTip = tip - joint.pos;
const vec3d relTarget = target - joint.pos;
// calculate the required rotation
mat3d rot = calcDirectRotation(relTip, relTarget);
if (rot == mat3d(1.0)) return tip;
// apply constraints to the bone's orientation
if (mApplyConstraints)
{
mat3d oldRot = bs.rot;
bs.rot = bs.rot * rot;
applyConstraints(b, joint);
rot = transpose(oldRot) * bs.rot;
}
else // alternatively, just apply the rotation directly
bs.rot = bs.rot * rot;
return joint.pos + rot*relTip;
}
