Example #1
0
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;
        }
      }
    }
Example #2
0
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;
			}
		}
		}