void Wiimote::GetIRData(u8* const data, bool use_accel)
{
u16 x[4], y[4];
memset(x, 0xFF, sizeof(x));
ControlState xx = 10000, yy = 0, zz = 0;
double nsin, ncos;
if (use_accel)
{
double ax, az, len;
ax = m_accel.x;
az = m_accel.z;
len = sqrt(ax * ax + az * az);
if (len)
{
ax /= len;
az /= len; // normalizing the vector
nsin = ax;
ncos = az;
}
else
{
nsin = 0;
ncos = 1;
}
}
else
{
// TODO m_tilt stuff
nsin = 0;
ncos = 1;
}
LowPassFilter(ir_sin, nsin, 1.0 / 60);
LowPassFilter(ir_cos, ncos, 1.0 / 60);
m_ir->GetState(&xx, &yy, &zz, true);
Vertex v[4];
static const int camWidth = 1024;
static const int camHeight = 768;
static const double bndup = -0.315447;
static const double bnddown = 0.85;
static const double bndleft = 0.443364;
static const double bndright = -0.443364;
static const double dist1 = 100.0 / camWidth; // this seems the optimal distance for zelda
static const double dist2 = 1.2 * dist1;
for (auto& vtx : v)
{
vtx.x = xx * (bndright - bndleft) / 2 + (bndleft + bndright) / 2;
if (m_sensor_bar_on_top)
vtx.y = yy * (bndup - bnddown) / 2 + (bndup + bnddown) / 2;
else
vtx.y = yy * (bndup - bnddown) / 2 - (bndup + bnddown) / 2;
vtx.z = 0;
}
v[0].x -= (zz * 0.5 + 1) * dist1;
v[1].x += (zz * 0.5 + 1) * dist1;
v[2].x -= (zz * 0.5 + 1) * dist2;
v[3].x += (zz * 0.5 + 1) * dist2;
#define printmatrix(m) \
PanicAlert("%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n", m[0][0], m[0][1], m[0][2], \
m[0][3], m[1][0], m[1][1], m[1][2], m[1][3], m[2][0], m[2][1], m[2][2], m[2][3], \
m[3][0], m[3][1], m[3][2], m[3][3])
Matrix rot, tot;
static Matrix scale;
MatrixScale(scale, 1, camWidth / camHeight, 1);
MatrixRotationByZ(rot, ir_sin, ir_cos);
MatrixMultiply(tot, scale, rot);
for (int i = 0; i < 4; i++)
{
MatrixTransformVertex(tot, v[i]);
if ((v[i].x < -1) || (v[i].x > 1) || (v[i].y < -1) || (v[i].y > 1))
continue;
x[i] = (u16)lround((v[i].x + 1) / 2 * (camWidth - 1));
y[i] = (u16)lround((v[i].y + 1) / 2 * (camHeight - 1));
}
// Fill report with valid data when full handshake was done
if (m_reg_ir.data[0x30])
// ir mode
switch (m_reg_ir.mode)
{
// basic
case 1:
{
memset(data, 0xFF, 10);
wm_ir_basic* const irdata = reinterpret_cast<wm_ir_basic*>(data);
for (unsigned int i = 0; i < 2; ++i)
{
if (x[i * 2] < 1024 && y[i * 2] < 768)
{
irdata[i].x1 = static_cast<u8>(x[i * 2]);
irdata[i].x1hi = x[i * 2] >> 8;
irdata[i].y1 = static_cast<u8>(y[i * 2]);
irdata[i].y1hi = y[i * 2] >> 8;
}
if (x[i * 2 + 1] < 1024 && y[i * 2 + 1] < 768)
{
irdata[i].x2 = static_cast<u8>(x[i * 2 + 1]);
irdata[i].x2hi = x[i * 2 + 1] >> 8;
irdata[i].y2 = static_cast<u8>(y[i * 2 + 1]);
irdata[i].y2hi = y[i * 2 + 1] >> 8;
}
}
}
void Wiimote::GetIRData(u8* const data, bool use_accel)
{
const bool has_focus = HAS_FOCUS;
u16 x[4], y[4];
memset(x, 0xFF, sizeof(x));
if (has_focus)
{
float xx = 10000, yy = 0, zz = 0;
double nsin,ncos;
if (use_accel)
{
double ax,az,len;
ax=m_accel.x;
az=m_accel.z;
len=sqrt(ax*ax+az*az);
if (len)
{
ax/=len;
az/=len; //normalizing the vector
nsin=ax;
ncos=az;
}
else
{
nsin=0;
ncos=1;
}
// PanicAlert("%d %d %d\nx:%f\nz:%f\nsin:%f\ncos:%f",accel->x,accel->y,accel->z,ax,az,sin,cos);
//PanicAlert("%d %d %d\n%d %d %d\n%d %d %d",accel->x,accel->y,accel->z,calib->zero_g.x,calib->zero_g.y,calib->zero_g.z,
// calib->one_g.x,calib->one_g.y,calib->one_g.z);
}
else
{
nsin=0; //m_tilt stuff here (can't figure it out yet....)
ncos=1;
}
LowPassFilter(ir_sin,nsin,1.0f/60);
LowPassFilter(ir_cos,ncos,1.0f/60);
m_ir->GetState(&xx, &yy, &zz, true);
UDPTLayer::GetIR(m_udp, &xx, &yy, &zz);
Vertex v[4];
static const int camWidth=1024;
static const int camHeight=768;
static const double bndup=-0.315447;
static const double bnddown=0.85;
static const double bndleft=0.443364;
static const double bndright=-0.443364;
static const double dist1=100.f/camWidth; //this seems the optimal distance for zelda
static const double dist2=1.2f*dist1;
for (auto& vtx : v)
{
vtx.x=xx*(bndright-bndleft)/2+(bndleft+bndright)/2;
if (m_sensor_bar_on_top) vtx.y=yy*(bndup-bnddown)/2+(bndup+bnddown)/2;
else vtx.y=yy*(bndup-bnddown)/2-(bndup+bnddown)/2;
vtx.z=0;
}
v[0].x-=(zz*0.5+1)*dist1;
v[1].x+=(zz*0.5+1)*dist1;
v[2].x-=(zz*0.5+1)*dist2;
v[3].x+=(zz*0.5+1)*dist2;
#define printmatrix(m) PanicAlert("%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n%f %f %f %f\n",m[0][0],m[0][1],m[0][2],m[0][3],m[1][0],m[1][1],m[1][2],m[1][3],m[2][0],m[2][1],m[2][2],m[2][3],m[3][0],m[3][1],m[3][2],m[3][3])
Matrix rot,tot;
static Matrix scale;
static bool isscale=false;
if (!isscale)
{
MatrixScale(scale,1,camWidth/camHeight,1);
//MatrixIdentity(scale);
}
MatrixRotationByZ(rot,ir_sin,ir_cos);
//MatrixIdentity(rot);
MatrixMultiply(tot,scale,rot);
for (int i=0; i<4; i++)
{
MatrixTransformVertex(tot,v[i]);
if ((v[i].x<-1)||(v[i].x>1)||(v[i].y<-1)||(v[i].y>1))
continue;
x[i]=(u16)round((v[i].x+1)/2*(camWidth-1));
y[i]=(u16)round((v[i].y+1)/2*(camHeight-1));
}
// PanicAlert("%f %f\n%f %f\n%f %f\n%f %f\n%d %d\n%d %d\n%d %d\n%d %d",
// v[0].x,v[0].y,v[1].x,v[1].y,v[2].x,v[2].y,v[3].x,v[3].y,
// x[0],y[0],x[1],y[1],x[2],y[2],x[3],y[38]);
}
// Fill report with valid data when full handshake was done
if (m_reg_ir.data[0x30])
// ir mode
switch (m_reg_ir.mode)
{
// basic
case 1 :
{
memset(data, 0xFF, 10);
wm_ir_basic* const irdata = (wm_ir_basic*)data;
for (unsigned int i=0; i<2; ++i)
{
if (x[i*2] < 1024 && y[i*2] < 768)
{
irdata[i].x1 = u8(x[i*2]);
irdata[i].x1hi = x[i*2] >> 8;
irdata[i].y1 = u8(y[i*2]);
irdata[i].y1hi = y[i*2] >> 8;
}
if (x[i*2+1] < 1024 && y[i*2+1] < 768)
{
irdata[i].x2 = u8(x[i*2+1]);
irdata[i].x2hi = x[i*2+1] >> 8;
irdata[i].y2 = u8(y[i*2+1]);
irdata[i].y2hi = y[i*2+1] >> 8;
}
}
}
