/*!
SLRay::diffuseMC scatters a ray around hit normal (cosine distribution).
This is only used for photonmapping(russian roulette).
The random direction lies around z-Axis and is then transformed by a rotation
matrix to lie along the normal. The direction is calculated according to MCCABE
*/
void SLRay::diffuseMC(SLRay* scattered)
{
SLVec3f randVec;
SLfloat eta1,eta2,eta1sqrt;
scattered->setDir(hitNormal);
scattered->origin = hitPoint;
scattered->depth = depth+1;
depthReached = scattered->depth;
// for reflectance the start material stays the same
scattered->originMat = hitMat;
scattered->originShape = hitShape;
scattered->type = REFLECTED;
//calculate rotation matrix
SLMat3f rotMat;
SLVec3f rotAxis((SLVec3f(0.0,0.0,1.0) ^ scattered->dir).normalize());
SLfloat rotAngle=acos(scattered->dir.z); //z*scattered.dir()
rotMat.rotation(rotAngle*180.0f/SL_PI, rotAxis);
//cosine distribution
eta1 = (SLfloat)random->Random();
eta2 = SL_2PI*(SLfloat)random->Random();
eta1sqrt = sqrt(1-eta1);
//transform to cartesian
randVec.set(eta1sqrt * cos(eta2),
eta1sqrt * sin(eta2),
sqrt(eta1));
scattered->setDir(rotMat*randVec);
}
bool urdf_traverser::jointTransformForAxis(const JointConstPtr& joint,
const Eigen::Vector3d& axis, Eigen::Quaterniond& rotation)
{
Eigen::Vector3d rotAxis(joint->axis.x, joint->axis.y, joint->axis.z);
if (rotAxis.norm() < 1e-06) return false;
rotAxis.normalize();
// ROS_INFO_STREAM("Rotation axis for joint "<<joint->name<<": "<<rotAxis);
if (equalAxes(rotAxis, axis, 1e-06)) return false;
//rotation = Eigen::Quaterniond::FromTwoVectors(rotAxis, axis);
//ROS_WARN_STREAM("z alignment from "<<rotAxis<<" to "<<axis<<" : "<<rotation);
rotation = Eigen::Quaterniond::FromTwoVectors(axis, rotAxis);
// ROS_WARN_STREAM("z alignment from "<<axis<<" to "<<rotAxis<<" : "<<rotation);
return true;
}
/// get the intersected piece from a ray in world space
int cRubikSnake::GetRayIntersection( TSRVector3& raySource, TSRVector3& rayDirection )
{
float Dist = FLT_MAX;
float newDist = 0.0f;
TSRMatrix4 currTransform;
currTransform.MakeIdent();
int iPossibleMoveIndex = -1;
float dummyT;
TSRVector3 q;
TSRMatrixStack stack;
stack.LoadIdentity();
stack.Push();
stack.TranslateLocal( m_vPanTranslation.x, m_vPanTranslation.y, m_vPanTranslation.z );
stack.MultMatrix( m_ArcBall.m_Transform.d );
stack.TranslateLocal( -m_Center.x, -m_Center.y, -m_Center.z );
TSRVector3 rotAxis( 1.0f, 0.0f, 0.0f );
for ( unsigned int i = 0; i < m_Angles.size(); i++ )
{
stack.Push();
stack.TranslateLocal( 0.25f, 0.25f, 0.0f );
memcpy( currTransform.d, stack.GetTop(), sizeof( TSRMatrix4 ) );
stack.Pop();
TSRVector3 boxSize( 0.6f, 0.6f, 0.6f );
stack.MultMatrix( m_Transform.d );
if ( TSRCollisionTests::IntersectRayOBB( raySource, rayDirection, currTransform, boxSize, dummyT, q ) )
{
newDist = ( raySource - q ).Mag();
if ( newDist < Dist )
{
Dist = newDist;
iPossibleMoveIndex = i;
m_SelectedIndex = i;
}
}
stack.TranslateLocal( -0.5f, 0.5f, 0.0f );
stack.RotateAxisLocal( rotAxis, m_Angles[ i ] * PI / 180.0f );
}
stack.Pop();
return iPossibleMoveIndex;
}
/*!
Photons are scattered (or absorbed) according to surface properties.
This is done by russian roulette.
Photons are stored on diffuse surfaces only.
*/
void SLPhotonMapper::photonScatter(SLRay* photon,
SLVec3f power,
SLPhotonType photonType)//, SLint RGB)
{
SLScene* s = SLScene::current; // scene shortcut
s->root3D()->hit(photon);
if (photon->length < SL_FLOAT_MAX)
{
//photons "absorbed" by luminaire
if (typeid(*photon->hitShape)==typeid(SLLightSphere) ||
typeid(*photon->hitShape)==typeid(SLLightRect))
return;
//abort if maps are full or depth>100
if((_mapCaustic->isFull() && _mapGlobal->isFull()) ||
photon->depth>100) //physically plausible ;-)
return;
photon->normalizeNormal();
SLMaterial* mat = photon->hitMat;
//store photon if diffuse surface and not from light
if (photon->nodeDiffuse() && photonType!=LIGHT)//photon->type()!=PRIMARY)
{
if(photonType!=CAUSTIC)
_mapGlobal->store(photon->hitPoint,photon->dir,power);
else
{
_mapCaustic->store(photon->hitPoint,photon->dir,power);
return; // caustic photons "die" on diffuse surfaces
}
}
//calculate average of materials
SLfloat avgDiffuse = (mat->diffuse().x+mat->diffuse().y+mat->diffuse().z)/3.0f;
SLfloat avgSpecular = (mat->specular().x+mat->specular().y+mat->specular().z)/3.0f;
SLfloat avgTransmission= (mat->transmission().x+mat->transmission().y+mat->transmission().z)/3.0f;
SLfloat eta = _russianRandom->Random();
//Decide type of photon (Global or Caustic) if from light
if (photonType == LIGHT)
{
if ((eta*(avgDiffuse+avgSpecular+avgTransmission))<=avgDiffuse)
photonType=GLOBAL;
else
photonType=CAUSTIC;
}
//Russian Roulette
if (eta <= avgDiffuse)
{
//scattered diffuse (cosine distribution around normal)
SLRay scattered;
photon->diffuseMC(&scattered);
//adjust power
power.x*=(mat->diffuse().x/avgDiffuse);
power.y*=(mat->diffuse().y/avgDiffuse);
power.z*=(mat->diffuse().z/avgDiffuse);
++SLRay::diffusePhotons;
photonScatter(&scattered, power, photonType);
}
else if (eta <= avgDiffuse+avgSpecular)
{
//scatter toward perfect specular direction
SLRay scattered;
photon->reflect(&scattered);
//scatter around perfect reflected direction only if material not perfect
if(photon->hitMat->shininess() < SLMaterial::PERFECT)
{
//rotation matrix
SLMat3f rotMat;
SLVec3f rotAxis((SLVec3f(0.0,0.0,1.0) ^ scattered.dir).normalize());
SLfloat rotAngle=acos(scattered.dir.z);//z*scattered.dir()
rotMat.rotation(rotAngle*180.0/SL_PI,rotAxis);
photon->reflectMC(&scattered,rotMat);
}
//avoid scattering into surface
if (scattered.dir*photon->hitNormal >= 0.0f)
{
//adjust power
power.x*=(mat->specular().x/avgSpecular);
power.y*=(mat->specular().y/avgSpecular);
power.z*=(mat->specular().z/avgSpecular);
++SLRay::reflectedPhotons;
photonScatter(&scattered,power,photonType);
}
}
else if (eta <= avgDiffuse+avgSpecular+avgTransmission) //scattered refracted
{
//scatter toward perfect transmissive direction
SLRay scattered;
photon->refract(&scattered);
//scatter around perfect transmissive direction only if material not perfect
if(photon->hitMat->translucency() < SLMaterial::PERFECT)
{
//rotation matrix
SLMat3f rotMat;
SLVec3f rotAxis((SLVec3f(0.0,0.0,1.0) ^ scattered.dir).normalize());
SLfloat rotAngle=acos(scattered.dir.z);//z*scattered.dir()
rotMat.rotation(rotAngle*180.0/SL_PI,rotAxis);
photon->refractMC(&scattered,rotMat);
}
SLVec3f N = -photon->hitNormal;
if(scattered.type==REFLECTED) N*=-1.0; //In case of total reflection invert the Normal
if (scattered.dir*N >= 0.0f)
{
//adjust power
power.x*=(mat->transmission().x/avgTransmission);
power.y*=(mat->transmission().y/avgTransmission);
power.z*=(mat->transmission().z/avgTransmission);
if(scattered.type==TRANSMITTED) ++SLRay::refractedPhotons; else ++SLRay::tirPhotons;
photonScatter(&scattered,power,photonType);
}
}
else //absorbed [rest in peace]
{
}
}
}
