void hgemitter::SetDirectionFromAngle( int flag, VECTOR *res, VECTOR *ang )
{
float spd;
VECTOR v;
spd = speed;
if ( spd != 0.0f ) spd += ((float)(rand()&4095) * FRND_4096 ) * speedopt;
if ( flag == HGMODEL_FLAG_2DSPRITE ) {
SetVector( res, sin(ang->z) * spd, cos(ang->z) * spd, 0.0f, 0.0f );
return;
}
SetVector( &v, 0.0f, 0.0f, spd, 0.0f );
InitMatrix();
switch( rotorder ) {
case HGMODEL_ROTORDER_ZYX:
RotZ( ang->z );
RotY( ang->y );
RotX( ang->x );
break;
case HGMODEL_ROTORDER_XYZ:
RotX( ang->x );
RotY( ang->y );
RotZ( ang->z );
break;
case HGMODEL_ROTORDER_YXZ:
RotY( ang->y );
RotX( ang->x );
RotZ( ang->z );
break;
}
ApplyMatrix( res, &v );
}
void OBBMeshQuery::PerformTest()
{
RenderTerrain();
Matrix3x3 MX,MY,MZ;
RotX(MX, mAngleX);
RotY(MY, mAngleY);
RotY(MZ, mAngleZ);
mBox.mRot = MX * MY * MZ;
DrawOBB(mBox);
const Model* TM = GetTerrainModel();
if(TM)
{
OBBCollider Collider;
mSettings.SetupCollider(Collider);
mProfiler.Start();
bool Status = Collider.Collide(mCache, mBox, *TM, null, null);
mProfiler.End();
mProfiler.Accum();
if(Status)
{
if(Collider.GetContactStatus())
{
udword NbTris = Collider.GetNbTouchedPrimitives();
const udword* Indices = Collider.GetTouchedPrimitives();
RenderTerrainTriangles(NbTris, Indices);
}
}
}
// Raycast hit
if(mValidHit)
{
Point wp = mLocalHit + mBox.mCenter;
DrawLine(wp, wp + Point(1.0f, 0.0f, 0.0f), Point(1,0,0), 1.0f);
DrawLine(wp, wp + Point(0.0f, 1.0f, 0.0f), Point(0,1,0), 1.0f);
DrawLine(wp, wp + Point(0.0f, 0.0f, 1.0f), Point(0,0,1), 1.0f);
}
char Buffer[4096];
sprintf(Buffer, "OBB-mesh query = %5.1f us (%d cycles)\n", mProfiler.mMsTime, mProfiler.mCycles);
GLFontRenderer::print(10.0f, 10.0f, 0.02f, Buffer);
}
/*
void AnObject::SetAttrib(float rd, float gr, float bl, Color a, float d, float b, float s, float ro, float r)
{
properties.SetAttrib(rd, gr, bl, a, d, b, s, ro, r);
}
*/
void AnObject::RotXYZ(double ax, double ay, double az)
{
AnObject *o;
switch(type) {
case OBJ_CONE: return ((ACone *)this)->RotXYZ(ax,ay,az);
case OBJ_CYLINDER: return ((ACylinder *)this)->RotXYZ(ax,ay,az);
case OBJ_INTERSECTION:
o = obj;
while (o) {
o->RotXYZ(ax,ay,az);
o = o->next;
}
break;
default:
RotX(ax);
RotY(ay);
RotZ(az);
o = obj;
while (o) {
o->RotXYZ(ax,ay,az);
o = o->next;
}
}
}
bool FDirectInputJoystick::IsChangedKeyState()const
{
// UE_LOG(LogDirectInputPadPlugin, Log, TEXT("x %d %d"), InitialJoyBuf_.lX, joyBuf_[nCurIndex_].lX);
// UE_LOG(LogDirectInputPadPlugin, Log, TEXT("y %d %d"), joyBuf_[nCurIndex_].lY, InitialJoyBuf_.lY);
// UE_LOG(LogDirectInputPadPlugin, Log, TEXT("z %d %d"), joyBuf_[nCurIndex_].lZ, InitialJoyBuf_.lZ);
// UE_LOG(LogDirectInputPadPlugin, Log, TEXT("Rx %d %d"), joyBuf_[nCurIndex_].lRx, InitialJoyBuf_.lRx);
// UE_LOG(LogDirectInputPadPlugin, Log, TEXT("Ry %d %d"), joyBuf_[nCurIndex_].lRy, InitialJoyBuf_.lRy);
// UE_LOG(LogDirectInputPadPlugin, Log, TEXT("Rz %d %d"), joyBuf_[nCurIndex_].lRz, InitialJoyBuf_.lRz);
// 軸からチェック。初期値と比較
if(InitX()!=X()
|| InitY()!=Y()
|| InitZ()!=Z()
|| InitRotX()!=RotX()
|| InitRotY()!=RotY()
|| InitRotZ()!=RotZ())
return true;
// ボタンは、PushもReleaseもされてない
for(uint32 i = 0; i<32; ++i)
{
if(((joyBuf_[nCurIndex_].rgbButtons[i]&0x80)>0)||((joyBuf_[1^nCurIndex_].rgbButtons[i]&0x80)>0))
return true;
}
// POVも押されていない
if(LOWORD(joyBuf_[nCurIndex_].rgdwPOV[0])!=0xFFFF) return true;
//UE_LOG(LogDirectInputPadPlugin, Log, TEXT("IsChangedState: false"));
return false;
}
void PyramidObject::transform( void )
{
// update the angles
this->angles += this->rotSpeeds;
// create temp matrices
Matrix Scale(SCALE, 0.5f, 0.5f, 0.5f);
Matrix RotX( ROT_X, this->angles[x] );
Matrix RotY( ROT_Y, this->angles[y] );
Matrix RotZ( ROT_Z, this->angles[z] );
///////////////////////DEMO CODE, UNECESSARY REMOVE ///////////////////////////
if (this->currPos[x] <= -7.0f)
this->dir[x] *= -1;
if (this->currPos[x] >= -3.0f)
this->dir[x] *= -1;
/////////////////////////////////////////////////////////////////////////////
this->currPos[x] += this->dir[x];
this->currPos[y] += this->dir[y];
this->currPos[z] += this->dir[z];
Matrix Trans( TRANS, this->currPos[x], this->currPos[y], this->currPos[z]);
// Create the local to world matrix (ie Model)
this->LocalToWorld = Scale * RotY * RotX * RotZ * Trans;
// Create the ModelView ( LocalToWorld * View)
// Some pipelines have the project concatenated, others don't
// Best to keep the separated, you can always join them with a quick multiply
this->ModelView = this->LocalToWorld * pCamera->getViewMatrix();
};
void initialize(void)
{
//VP_BASIC_RENDERER_WORLD(world);
VP_FRAMEWORK_WORLD(world)
VP_FRAMEWORK_CAMERA(-49.9825, 15.968, 15.5653, -109.286, 78.4562, 0.767533)
vpMaterial::GetDefaultMaterial()->SetRestitution(0.2);
vpMaterial::GetDefaultMaterial()->SetDynamicFriction(1.0);
ground.SetGround();
ground.AddGeometry(new vpBox(Vec3(20, 20, 1)));
ground.AddGeometry(new vpCapsule(0.5, 11.0), Vec3(0, 0, 5));
ground.AddGeometry(new vpCapsule(0.5, 6.0), SE3(Vec3(0, -2.5, 10))*RotX(0.5*M_PI));
for ( int i = 0; i < NUM_CHAIN-1; i++ )
{
chain[i].SetJoint(&J[i], Vec3(10.0 / NUM_CHAIN, 0, 0));
chain[i+1].SetJoint(&J[i], Vec3(-10.0 / NUM_CHAIN, 0, 0));
//chain[i].AddGeometry(new vpBox(Vec3(20.0 / NUM_CHAIN, 0.2, 0.2)));
chain[i].AddGeometry(new vpCapsule(0.1, 20.0 / NUM_CHAIN + 0.2), RotY(0.5*M_PI));
for ( int j = -3; j < 3; j++ )
if ( i+j >= 0 && i+j <= NUM_CHAIN-1 ) world.IgnoreCollision(&chain[i], &chain[i+j]);
J[i].SetDamping(SpatialDamper(20));
J[i].SetElasticity(SpatialSpring(1500));
J[i].SetOrientation(Exp(Axis(0.01, 0.01, 0.01)));
chain[i].SetInertia(Inertia(.1));
}
chain[NUM_CHAIN-1].SetInertia(Inertia(1));
chain[NUM_CHAIN-1].AddGeometry(new vpCapsule(0.1, 20.0 / NUM_CHAIN + 0.2), RotY(0.5*M_PI));
chain[NUM_CHAIN/2].SetFrame(Vec3(0, -2, 15));
world.AddBody(&chain[NUM_CHAIN/2]);
world.AddBody(&ground);
world.SetGravity(Vec3(0.0, 0.0, -10.0));
world.Initialize();
world.SetIntegrator(VP::IMPLICIT_EULER_FAST);
world.SetTimeStep(0.005);
world.BackupState();
vpTimer timer;
timer.Tic();
for ( int i = 0; i < 1000; i++ ) world.StepAhead();
cout << timer.Toc() << chain[10].GetGenVelocity() << endl;
}
NjFloat4x4 NjFloat4x4::PYR( float _Pitch, float _Yaw, float _Roll )
{
NjFloat4x4 Pitch = RotX( _Pitch );
NjFloat4x4 Yaw = RotY( _Yaw );
NjFloat4x4 Roll = RotZ( _Roll );
NjFloat4x4 Result = Pitch * Yaw * Roll;
return Result;
}
void CapsuleMeshQuery::Init()
{
mP0 = Point(0.0f, -4.0f, 0.0f);
mP1 = Point(0.0f, 4.0f, 0.0f);
Matrix3x3 MX,MY;
RotX(MX, 45.0f);
RotY(MY, 45.0f);
mWorld = MX * MY;
mWorld.SetTrans(0.0f, 4.0f, 0.0f);
}
void APlane::Rotate(Vector rv)
{
if (usesTransform) {
Transform T;
T.CalcRotation(rv);
trans.Compose(&T);
return;
}
RotX(rv.x);
RotY(rv.y);
RotZ(rv.z);
}
/////////////////////////////
// イベント
/////////////////////////////
void FDirectInputJoystick::Event(const TSharedPtr<FGenericApplicationMessageHandler>& MessageHandler)
{
// JoystickMapをぶん回す
// 軸チェック
EventAnalog(MessageHandler, X(), DIGamePad_AXIS_X, EKeysDirectInputPad::DIGamePad_AxisX);
EventAnalog(MessageHandler, Y(), DIGamePad_AXIS_Y, EKeysDirectInputPad::DIGamePad_AxisY);
EventAnalog(MessageHandler, Z(), DIGamePad_AXIS_Z, EKeysDirectInputPad::DIGamePad_AxisZ);
EventAnalog(MessageHandler, RotX(), DIGamePad_ROT_X, EKeysDirectInputPad::DIGamePad_RotX);
EventAnalog(MessageHandler, RotY(), DIGamePad_ROT_Y, EKeysDirectInputPad::DIGamePad_RotY);
EventAnalog(MessageHandler, RotZ(), DIGamePad_ROT_Z, EKeysDirectInputPad::DIGamePad_RotZ);
EventPov(MessageHandler);
// ボタンチェック
EventButton(MessageHandler, DIGamePad_Button1, EKeysDirectInputPad::DIGamePad_Button1);
EventButton(MessageHandler, DIGamePad_Button2, EKeysDirectInputPad::DIGamePad_Button2);
EventButton(MessageHandler, DIGamePad_Button3, EKeysDirectInputPad::DIGamePad_Button3);
EventButton(MessageHandler, DIGamePad_Button4, EKeysDirectInputPad::DIGamePad_Button4);
EventButton(MessageHandler, DIGamePad_Button5, EKeysDirectInputPad::DIGamePad_Button5);
EventButton(MessageHandler, DIGamePad_Button6, EKeysDirectInputPad::DIGamePad_Button6);
EventButton(MessageHandler, DIGamePad_Button7, EKeysDirectInputPad::DIGamePad_Button7);
EventButton(MessageHandler, DIGamePad_Button8, EKeysDirectInputPad::DIGamePad_Button8);
EventButton(MessageHandler, DIGamePad_Button9, EKeysDirectInputPad::DIGamePad_Button9);
EventButton(MessageHandler, DIGamePad_Button10, EKeysDirectInputPad::DIGamePad_Button10);
EventButton(MessageHandler, DIGamePad_Button11, EKeysDirectInputPad::DIGamePad_Button11);
EventButton(MessageHandler, DIGamePad_Button12, EKeysDirectInputPad::DIGamePad_Button12);
EventButton(MessageHandler, DIGamePad_Button13, EKeysDirectInputPad::DIGamePad_Button13);
EventButton(MessageHandler, DIGamePad_Button14, EKeysDirectInputPad::DIGamePad_Button14);
EventButton(MessageHandler, DIGamePad_Button15, EKeysDirectInputPad::DIGamePad_Button15);
EventButton(MessageHandler, DIGamePad_Button16, EKeysDirectInputPad::DIGamePad_Button16);
EventButton(MessageHandler, DIGamePad_Button17, EKeysDirectInputPad::DIGamePad_Button17);
EventButton(MessageHandler, DIGamePad_Button18, EKeysDirectInputPad::DIGamePad_Button18);
EventButton(MessageHandler, DIGamePad_Button19, EKeysDirectInputPad::DIGamePad_Button19);
EventButton(MessageHandler, DIGamePad_Button20, EKeysDirectInputPad::DIGamePad_Button20);
EventButton(MessageHandler, DIGamePad_Button21, EKeysDirectInputPad::DIGamePad_Button21);
EventButton(MessageHandler, DIGamePad_Button22, EKeysDirectInputPad::DIGamePad_Button22);
EventButton(MessageHandler, DIGamePad_Button23, EKeysDirectInputPad::DIGamePad_Button23);
EventButton(MessageHandler, DIGamePad_Button24, EKeysDirectInputPad::DIGamePad_Button24);
EventButton(MessageHandler, DIGamePad_Button25, EKeysDirectInputPad::DIGamePad_Button25);
EventButton(MessageHandler, DIGamePad_Button26, EKeysDirectInputPad::DIGamePad_Button26);
EventButton(MessageHandler, DIGamePad_Button27, EKeysDirectInputPad::DIGamePad_Button27);
EventButton(MessageHandler, DIGamePad_Button28, EKeysDirectInputPad::DIGamePad_Button28);
EventButton(MessageHandler, DIGamePad_Button29, EKeysDirectInputPad::DIGamePad_Button29);
EventButton(MessageHandler, DIGamePad_Button30, EKeysDirectInputPad::DIGamePad_Button30);
EventButton(MessageHandler, DIGamePad_Button31, EKeysDirectInputPad::DIGamePad_Button31);
EventButton(MessageHandler, DIGamePad_Button32, EKeysDirectInputPad::DIGamePad_Button32);
}
void Camera::Orbit( void )
{
Vect CamFocusPos;
Vect focus;
focus.set(0,2.5f,-18.0f);
CamFocusPos = this->vPos - focus;
float angle_y = 0.001f;
Matrix RotY( ROT_Y, angle_y );
CamFocusPos *= RotY;
CamFocusPos += focus;
this->setOrientAndPosition( Vect(0.0f,1.0f,0.0f), Vect(0.0f,2.5f,-18.0f), CamFocusPos );
}
Matrix LibOVR_Adaptor::getViewMatrixAfterMovement()
{
//float headBaseToEyeHeight = 0.15f; // Vertical height of eye from base of head
//float headBaseToEyeProtrusion = 0.09f; // Distance forward of eye from base of head
//OVR::Matrix4f rollPitchYaw = OVR::Matrix4f::RotationY(EyeYaw) * OVR::Matrix4f::RotationX(EyePitch) * OVR::Matrix4f::RotationZ(EyeRoll);
//OVR::Vector3f up = rollPitchYaw.Transform( OVR::Vector3f(0, Up[y], Up[z]));
//OVR::Vector3f forward = rollPitchYaw.Transform( OVR::Vector3f(Dir[x], Dir[y], Dir[z]));
OVR::Vector3f eyeCenterInHeadFrame(0.0f, 0.15f, -0.09f);
OVR::Vector3f shiftedEyePos = this->EyePos + eyeCenterInHeadFrame;
EyePos.y -= eyeCenterInHeadFrame.y; // Bring the head back down to original height
//OVR::Matrix4f View = OVR::Matrix4f::LookAtRH(shiftedEyePos, shiftedEyePos + forward, up);
Matrix RotX( ROT_X, this->EyePitch * -1);
Matrix RotY( ROT_Y, (this->EyeYaw + 180) * -1 );
Matrix RotZ( ROT_Z, this->EyeRoll * -1 );
Matrix Trans( TRANS, this->EyePos.x, this->EyePos.y, this->EyePos.z);
return (RotY * RotX * RotZ);// * Trans);
}
static int MakeHedron(int np, int dp, int nq, int dq, double cp, double cq) {
STri t0;
double pang,qang,cosp,cosq,sinp,sinq;
#ifdef DBG
printf("MakeHedron: p = %d/%d q = %d/%d cp = %.4f cq = %.4f\n",np,dp,nq,dq,cp,cq);
#endif
nregtris = nfaces = nverts = 0;
center_verts = state.verts>0?1:0;
subdiv_sides = state.verts==2?1:0;
nsides[0] = 4;
nsides[1] = np;
nsides[2] = nq;
/* these are two of the angles of a right spherical triangle */
pang = PI*(double)dp/(double)np;
qang = PI*(double)dq/(double)nq;
/* compute the (sin,cos) of sides of the spherical triangle */
cosp = cos(pang)/sin(qang);
cosq = cos(qang)/sin(pang);
sinp = sqrt(1.0-cosp*cosp);
sinq = sqrt(1.0-cosq*cosq);
t0.p[0].x = 0.0; t0.p[0].y = 0.0; t0.p[0].z = 1.0;
t0.p[1].x = sinq; t0.p[1].y = 0.0; t0.p[1].z = cosq;
t0.p[2].x = 0.0; t0.p[2].y = sinp; t0.p[2].z = cosp;
t0.v.x = cq*sinq;
t0.v.y = cp*sinp;
t0.v.z = 1.0 + cq*(cosq-1.0) + cp*(cosp-1.0);
NormVect(&t0.v.x);
#ifdef DBG
printf(" pang = %.5f, qang = %.5f \n", RadToDeg(pang), RadToDeg(qang));
printf(" cpa = %.5f, cqa = %.5f\n", RadToDeg(acos(cosp)),RadToDeg(acos(cosq)));
printf(" t = (%.4f,%.4f,%.4f) \n",t0.v.x,t0.v.y,t0.v.z);
#endif
switch(state.axis) {
case 1: /* want P aligned with Z -- rotate into pos*/
RotY(&t0.p[0],-sinq,cosq);
RotY(&t0.p[1],-sinq,cosq);
RotY(&t0.p[2],-sinq,cosq);
RotY(&t0.v,-sinq,cosq);
break;
case 2: /* Q aligned with Z -- */
RotX(&t0.p[0],sinp,cosp);
RotX(&t0.p[1],sinp,cosp);
RotX(&t0.p[2],sinp,cosp);
RotX(&t0.v,sinp,cosp);
break;
}
#ifdef DBG
printf(" p0 = %.4f, %.4f, %.4f \n",t0.p[0].x,t0.p[0].y,t0.p[0].z);
printf(" p1 = %.4f, %.4f, %.4f \n",t0.p[1].x,t0.p[1].y,t0.p[1].z);
printf(" p2 = %.4f, %.4f, %.4f \n",t0.p[2].x,t0.p[2].y,t0.p[2].z);
printf(" v = %.4f, %.4f, %.4f \n",t0.v.x,t0.v.y,t0.v.z);
#endif
memset(axis,0,3*sizeof(AxisList));
reg_vert(&t0.v);
level = 0;
traverse(&t0, -1);
do_axis[0] = do_axis[1] = do_axis[2] = 1;
star_axis[0] = star_axis[1] = star_axis[2] = 0;
if (dp>1) star_axis[1] = 1;
if (dq>1) star_axis[2] = 1;
if (cp==0||cq==0) do_axis[0] = 0;
if (cp==0.0 && cq==1.0) do_axis[1] = 0;
if (cp==1.0 && cq==0.0) do_axis[2] = 0;
#ifdef DBG
printf(" nsides = %d,%d,%d \n",nsides[0],nsides[1],nsides[2]);
printf(" do_axis= %d,%d,%d \n",do_axis[0],do_axis[1],do_axis[2]);
printf(" numaxis = %d,%d,%d \n",axis[0].num,axis[1].num,axis[2].num);
printf(" nverts = %d \n",nverts);
printf(" ------------------ MakeHedron done -----------------\n");
#endif
return(1);
}
void Renderer::Sphere(int del_uhol_x, int del_uhol_y, float size)
{
// del_uhol_x krat 2*(del_uhol_y+1) bodov
// Vertex_t v[del_uhol_x*2*(del_uhol_y+1)];
PosColNormalUVVertex* v = new PosColNormalUVVertex[del_uhol_x*2*(del_uhol_y+1)];
D3DXVECTOR3 a,b,ay,by;
float dy=360.f/del_uhol_y;
float dx=180.f*PI180/del_uhol_x;
for(int x=0; x<del_uhol_x; x++)
{
float tx = (float)x/(float)del_uhol_x;
float ang = (tx*180.f-90.f)*PI180; // <-90, 90)
a = D3DXVECTOR3( 0, (float)sin(ang), (float)cos(ang));
b = D3DXVECTOR3( 0, (float)sin(ang+dx), (float)cos(ang+dx));
// v[x*2*(del_uhol_y+1)+0].t0.set( 0, 1.f-tx);
v[x*2*(del_uhol_y+1)+0].Normal = a;
v[x*2*(del_uhol_y+1)+0].Coord = size*a;
// v[x*2*(del_uhol_y+1)+1].t0.set( 0, 1.f-(tx+1.f/del_uhol_x));
v[x*2*(del_uhol_y+1)+1].Normal= b;
v[x*2*(del_uhol_y+1)+1].Coord = size*b;
ay = a; by = b;
for(int y=1; y<del_uhol_y+1; y++)
{
float ty = (float)y/(float)del_uhol_y;
ay = a; RotY( ty*360.f, ay);
by = b; RotY( ty*360.f, by);
// v[x*2*(del_uhol_y+1)+2*y].t0.set( ty, 1.f-tx);
v[x*2*(del_uhol_y+1)+2*y].Normal = ay;
v[x*2*(del_uhol_y+1)+2*y].Coord = size*ay;
// v[x*2*(del_uhol_y+1)+2*y+1].t0.set( ty, 1.f-(tx+1.f/del_uhol_x));
v[x*2*(del_uhol_y+1)+2*y+1].Normal = by;
v[x*2*(del_uhol_y+1)+2*y+1].Coord = size*by;
}
}
DiffuseUVVertexShader* pShader = NULL;
if ( g_envMapSet )
{
pShader = &DiffuseUVEnvVertexShader::StaticType;
m_pD3DDevice->SetVertexDeclaration( g_pPosNormalColUVDeclaration );
}
else
{
pShader = &DiffuseUVVertexShader::StaticType;
m_pD3DDevice->SetVertexDeclaration( g_pPosColUVDeclaration );
}
CommitTransforms( pShader );
CommitTextureState();
m_pD3DDevice->SetVertexShader( pShader->GetVertexShader() );
for(int i=0; i<del_uhol_x; i++)
// g_device->DrawPrimitive(D3DPT_TRIANGLESTRIP, i*2*(del_y+1), 2*del_y );
m_pD3DDevice->DrawPrimitiveUP(D3DPT_TRIANGLESTRIP, 2*del_uhol_y, &v[i*2*(del_uhol_y+1)], sizeof(PosColNormalUVVertex));
delete v;
// pd->DrawPrimitive( D3DPT_TRIANGLESTRIP, i*2*(del_y+1), 2*del_y );
}
void Ambix_rotatorAudioProcessor::calcParams()
{
// use old sampling method for generating rotation matrix...
#if 0
if (!_initialized)
{
sph_h.Init(AMBI_ORDER);
const String t_design_txt (t_design::des_3_240_21_txt);
// std::cout << t_design_txt << std::endl;
String::CharPointerType lineChar = t_design_txt.getCharPointer();
int n = 0; // how many characters been read
int numsamples = 0;
int i = 0;
int curr_n = 0;
int max_n = lineChar.length();
while (curr_n < max_n) { // check how many coordinates we have
double value;
sscanf(lineChar, "%lf\n%n", &value, &n);
lineChar += n;
curr_n += n;
numsamples++;
} // end parse numbers
numsamples = numsamples/3; // xyz
Carth_coord.resize(numsamples,3); // positions in cartesian coordinates
curr_n = 0;
lineChar = t_design_txt.getCharPointer();
// parse line for numbers again and copy to carth coordinate matrix
while (i < numsamples) {
double x,y,z;
sscanf(lineChar, "%lf%lf%lf%n", &x, &y, &z, &n);
Carth_coord(i,0) = x;
Carth_coord(i,1) = y;
Carth_coord(i,2) = z;
lineChar += n;
curr_n += n;
i++;
} // end parse numbers
Sph_coord.resize(numsamples,2); // positions in spherical coordinates
Eigen::MatrixXd Sh_matrix(numsamples,AMBI_CHANNELS);
for (int i=0; i < numsamples; i++)
{
Eigen::VectorXd Ymn(AMBI_CHANNELS); // Ymn result
Sph_coord(i,0) = atan2(Carth_coord(i,1),Carth_coord(i,0)); // azimuth
Sph_coord(i,1) = atan2(Carth_coord(i,2),sqrt(Carth_coord(i,0)*Carth_coord(i,0) + Carth_coord(i,1)*Carth_coord(i,1))); // elevation
sph_h.Calc(Sph_coord(i,0),Sph_coord(i,1)); // phi theta
sph_h.Get(Ymn);
Sh_matrix.row(i) = Ymn;
}
// inversion would not be necessary because of t-design -> transpose is enough..
Sh_matrix_inv = (Sh_matrix.transpose()*Sh_matrix).inverse()*Sh_matrix.transpose();
_initialized = true;
}
Eigen::MatrixXd Sh_matrix_mod(Sph_coord.rows(),AMBI_CHANNELS);
// rotation parameters in radiants
// use mathematical negative angles for yaw
double yaw = -((double)yaw_param*2*M_PI - M_PI); // z
double pitch = (double)pitch_param*2*M_PI - M_PI; // y
double roll = (double)roll_param*2*M_PI - M_PI; // x
Eigen::Matrix3d RotX, RotY, RotZ, Rot;
RotX = RotY = RotZ = Eigen::Matrix3d::Zero(3,3);
RotX(0,0) = 1.f;
RotX(1,1) = RotX(2,2) = cos(roll);
RotX(1,2) = -sin(roll);
RotX(2,1) = -RotX(1,2);
RotY(0,0) = RotY(2,2) = cos(pitch);
RotY(0,2) = sin(pitch);
RotY(2,0) = -RotY(0,2);
RotY(1,1) = 1.f;
RotZ(0,0) = RotZ(1,1) = cos(yaw);
RotZ(0,1) = -sin(yaw);
RotZ(1,0) = -RotZ(0,1);
RotZ(2,2) = 1.f;
// multiply individual rotation matrices
if (rot_order_param < 0.5f)
{
// ypr order zyx -> mutliply inverse!
Rot = RotX * RotY * RotZ;
} else {
// rpy order xyz -> mutliply inverse!
Rot = RotZ * RotY * RotX;
}
// combined roll-pitch-yaw rotation matrix would be here
// http://planning.cs.uiuc.edu/node102.html
for (int i=0; i < Carth_coord.rows(); i++)
{
// rotate carthesian coordinates
Eigen::Vector3d Carth_coord_mod = Carth_coord.row(i)*Rot;
Eigen::Vector2d Sph_coord_mod;
// convert to spherical coordinates
Sph_coord_mod(0) = atan2(Carth_coord_mod(1),Carth_coord_mod(0)); // azimuth
Sph_coord_mod(1) = atan2(Carth_coord_mod(2),sqrt(Carth_coord_mod(0)*Carth_coord_mod(0) + Carth_coord_mod(1)*Carth_coord_mod(1))); // elevation
Eigen::VectorXd Ymn(AMBI_CHANNELS); // Ymn result
// calc spherical harmonic
sph_h.Calc(Sph_coord_mod(0),Sph_coord_mod(1)); // phi theta
sph_h.Get(Ymn);
// save to sh matrix
Sh_matrix_mod.row(i) = Ymn;
}
// calculate new transformation matrix
Sh_transf = Sh_matrix_inv * Sh_matrix_mod;
#else
// use
// Ivanic, J., Ruedenberg, K. (1996). Rotation Matrices for Real
// Spherical Harmonics. Direct Determination by Recursion.
// The Journal of Physical Chemistry
// rotation parameters in radiants
// use mathematical negative angles for yaw
double yaw = -((double)yaw_param*2*M_PI - M_PI); // z
double pitch = (double)pitch_param*2*M_PI - M_PI; // y
double roll = (double)roll_param*2*M_PI - M_PI; // x
Eigen::Matrix3d RotX, RotY, RotZ, Rot;
RotX = RotY = RotZ = Eigen::Matrix3d::Zero(3,3);
RotX(0,0) = 1.f;
RotX(1,1) = RotX(2,2) = cos(roll);
RotX(1,2) = sin(roll);
RotX(2,1) = -RotX(1,2);
RotY(0,0) = RotY(2,2) = cos(pitch);
RotY(0,2) = sin(pitch);
RotY(2,0) = -RotY(0,2);
RotY(1,1) = 1.f;
RotZ(0,0) = RotZ(1,1) = cos(yaw);
RotZ(0,1) = sin(yaw);
RotZ(1,0) = -RotZ(0,1);
RotZ(2,2) = 1.f;
// multiply individual rotation matrices
if (rot_order_param < 0.5f)
{
// ypr order zyx -> mutliply inverse!
Rot = RotX * RotY * RotZ;
} else {
// rpy order xyz -> mutliply inverse!
Rot = RotZ * RotY * RotX;
}
// first order initialization - prototype matrix
Eigen::Matrix3d R_1;
R_1(0,0) = Rot(1,1);
R_1(0,1) = Rot(1,2);
R_1(0,2) = Rot(1,0);
R_1(1,0) = Rot(2,1);
R_1(1,1) = Rot(2,2);
R_1(1,2) = Rot(2,0);
R_1(2,0) = Rot(0,1);
R_1(2,1) = Rot(0,2);
R_1(2,2) = Rot(0,0);
// zeroth order is invariant
Sh_transf(0,0) = 1.;
// set first order
Sh_transf.block(1, 1, 3, 3) = R_1;
Eigen::MatrixXd R_lm1 = R_1;
// recursivly generate higher orders
for (int l=2; l<=AMBI_ORDER; l++)
{
Eigen::MatrixXd R_l = Eigen::MatrixXd::Zero(2*l+1, 2*l+1);
for (int m=-l;m <= l; m++)
{
for (int n=-l;n <= l; n++)
{
// Table I
int d = (m==0) ? 1 : 0;
double denom = 0.;
if (abs(n) == l)
denom = (2*l)*(2*l-1);
else
denom = (l*l-n*n);
double u = sqrt((l*l-m*m)/denom);
double v = sqrt((1.+d)*(l+abs(m)-1.)*(l+abs(m))/denom)*(1.-2.*d)*0.5;
double w = sqrt((l-abs(m)-1.)*(l-abs(m))/denom)*(1.-d)*(-0.5);
if (u != 0.)
u *= U(l,m,n,R_1,R_lm1);
if (v != 0.)
v *= V(l,m,n,R_1,R_lm1);
if (w != 0.)
w *= W(l,m,n,R_1,R_lm1);
R_l(m+l,n+l) = u + v + w;
}
}
Sh_transf.block(l*l, l*l, 2*l+1, 2*l+1) = R_l;
R_lm1 = R_l;
}
#endif
// threshold coefficients
// maybe not needed here...
for (int i = 0; i < Sh_transf.size(); i++)
{
if (abs(Sh_transf(i)) < 0.00001f)
Sh_transf(i) = 0.f;
}
}