mirror of
https://github.com/JulianGro/overte.git
synced 2025-04-25 21:15:07 +02:00
239 lines
No EOL
7.8 KiB
C++
239 lines
No EOL
7.8 KiB
C++
//
|
|
// Hand.cpp
|
|
// interface
|
|
//
|
|
// Created by Philip Rosedale on 10/13/12.
|
|
// Copyright (c) 2012 High Fidelity, Inc. All rights reserved.
|
|
//
|
|
|
|
#include "Hand.h"
|
|
#include <sys/time.h>
|
|
|
|
const float PHI = 1.618;
|
|
|
|
const float DEFAULT_X = 0;
|
|
const float DEFAULT_Y = -1.5;
|
|
const float DEFAULT_Z = 2.0;
|
|
const float DEFAULT_TRANSMITTER_HZ = 60.0;
|
|
|
|
Hand::Hand(glm::vec3 initcolor)
|
|
{
|
|
color = initcolor;
|
|
reset();
|
|
noise = 0.0; //0.2;
|
|
scale.x = 0.07;
|
|
scale.y = scale.x * 5.0;
|
|
scale.z = scale.y * 1.0;
|
|
renderPointer = true;
|
|
}
|
|
|
|
Hand::Hand(const Hand &otherHand) {
|
|
color = otherHand.color;
|
|
noise = otherHand.noise;
|
|
scale = otherHand.scale;
|
|
position = otherHand.position;
|
|
target = otherHand.target;
|
|
velocity = otherHand.color;
|
|
pitch = otherHand.pitch;
|
|
yaw = otherHand.yaw;
|
|
roll = otherHand.roll;
|
|
pitchRate = otherHand.pitchRate;
|
|
yawRate = otherHand.yawRate;
|
|
rollRate = otherHand.rollRate;
|
|
transmitterTimer = otherHand.transmitterTimer;
|
|
transmitterHz = otherHand.transmitterHz;
|
|
transmitterPackets = otherHand.transmitterPackets;
|
|
renderPointer = otherHand.renderPointer;
|
|
}
|
|
|
|
void Hand::reset()
|
|
{
|
|
position.x = DEFAULT_X;
|
|
position.y = DEFAULT_Y;
|
|
position.z = DEFAULT_Z;
|
|
pitch = yaw = roll = 0;
|
|
pitchRate = yawRate = rollRate = 0;
|
|
setTarget(position);
|
|
velocity.x = velocity.y = velocity.z = 0;
|
|
transmitterPackets = 0;
|
|
transmitterHz = DEFAULT_TRANSMITTER_HZ;
|
|
}
|
|
|
|
void Hand::render(int isMine)
|
|
{
|
|
const float POINTER_LENGTH = 20.0;
|
|
glPushMatrix();
|
|
glTranslatef(position.x, position.y, position.z);
|
|
glRotatef(yaw, 0, 1, 0);
|
|
glRotatef(pitch, 1, 0, 0);
|
|
glRotatef(roll, 0, 0, 1);
|
|
glColor3f(color.x, color.y, color.z);
|
|
glScalef(scale.x, scale.y, scale.z);
|
|
//glutSolidSphere(1.5, 20, 20);
|
|
glutSolidCube(1.0);
|
|
if (renderPointer) {
|
|
glBegin(GL_TRIANGLES);
|
|
glColor3f(1,0,0);
|
|
glNormal3f(0,-1,0);
|
|
glVertex3f(-0.4,0,0);
|
|
glVertex3f(0.4,0,0);
|
|
glVertex3f(0,0,-POINTER_LENGTH);
|
|
glEnd();
|
|
glPushMatrix();
|
|
glTranslatef(0,0,-POINTER_LENGTH);
|
|
glutSolidCube(1.0);
|
|
glPopMatrix();
|
|
}
|
|
glPopMatrix();
|
|
|
|
if (1) {
|
|
// Render debug info from the transmitter
|
|
/*
|
|
glPushMatrix();
|
|
glMatrixMode(GL_PROJECTION);
|
|
glLoadIdentity();
|
|
//gluOrtho2D(0, WIDTH, HEIGHT, 0);
|
|
glDisable(GL_DEPTH_TEST);
|
|
glDisable(GL_LIGHTING);
|
|
glPopMatrix();
|
|
*/
|
|
|
|
}
|
|
|
|
}
|
|
|
|
void Hand::addAngularVelocity (float pRate, float yRate, float rRate) {
|
|
pitchRate += pRate;
|
|
yawRate += yRate;
|
|
rollRate += rRate;
|
|
}
|
|
|
|
void Hand::processTransmitterData(char *packetData, int numBytes) {
|
|
// Read a packet from a transmitter app, process the data
|
|
float accX, accY, accZ,
|
|
graX, graY, graZ,
|
|
gyrX, gyrY, gyrZ,
|
|
linX, linY, linZ,
|
|
rot1, rot2, rot3, rot4;
|
|
sscanf((char *)packetData, "tacc %f %f %f gra %f %f %f gyr %f %f %f lin %f %f %f rot %f %f %f %f",
|
|
&accX, &accY, &accZ,
|
|
&graX, &graY, &graZ,
|
|
&gyrX, &gyrY, &gyrZ,
|
|
&linX, &linY, &linZ,
|
|
&rot1, &rot2, &rot3, &rot4);
|
|
|
|
if (transmitterPackets++ == 0) {
|
|
gettimeofday(&transmitterTimer, NULL);
|
|
}
|
|
const int TRANSMITTER_COUNT = 100;
|
|
if (transmitterPackets % TRANSMITTER_COUNT == 0) {
|
|
// Every 100 packets, record the observed Hz of the transmitter data
|
|
timeval now;
|
|
gettimeofday(&now, NULL);
|
|
double msecsElapsed = diffclock(&transmitterTimer, &now);
|
|
transmitterHz = (float)TRANSMITTER_COUNT/(msecsElapsed/1000.0);
|
|
//std::cout << "Transmitter Hz: " << (float)TRANSMITTER_COUNT/(msecsElapsed/1000.0) << "\n";
|
|
//memcpy(&transmitterTimer, &now, sizeof(timeval));
|
|
transmitterTimer = now;
|
|
}
|
|
// Add rotational forces to the hand
|
|
const float ANG_VEL_SENSITIVITY = 4.0;
|
|
const float ANG_VEL_THRESHOLD = 0.0;
|
|
float angVelScale = ANG_VEL_SENSITIVITY*(1.0/getTransmitterHz());
|
|
//addAngularVelocity(gyrX*angVelScale,gyrZ*angVelScale,-gyrY*angVelScale);
|
|
addAngularVelocity(fabs(gyrX*angVelScale)>ANG_VEL_THRESHOLD?gyrX*angVelScale:0,
|
|
fabs(gyrZ*angVelScale)>ANG_VEL_THRESHOLD?gyrZ*angVelScale:0,
|
|
fabs(-gyrY*angVelScale)>ANG_VEL_THRESHOLD?-gyrY*angVelScale:0);
|
|
|
|
// Add linear forces to the hand
|
|
//const float LINEAR_VEL_SENSITIVITY = 50.0;
|
|
const float LINEAR_VEL_SENSITIVITY = 5.0;
|
|
float linVelScale = LINEAR_VEL_SENSITIVITY*(1.0/getTransmitterHz());
|
|
glm::vec3 linVel(linX*linVelScale, linZ*linVelScale, -linY*linVelScale);
|
|
addVelocity(linVel);
|
|
|
|
}
|
|
|
|
void Hand::simulate(float deltaTime)
|
|
{
|
|
const float ANGULAR_SPRING_CONSTANT = 0.25;
|
|
const float ANGULAR_DAMPING_COEFFICIENT = 5*2.0*powf(ANGULAR_SPRING_CONSTANT,0.5);
|
|
const float LINEAR_SPRING_CONSTANT = 100;
|
|
const float LINEAR_DAMPING_COEFFICIENT = 2.0*powf(LINEAR_SPRING_CONSTANT,0.5);
|
|
|
|
// If noise, add a bit of random velocity
|
|
const float RNOISE = 0.0;
|
|
const float VNOISE = 0.01;
|
|
if (noise) {
|
|
|
|
glm::vec3 nVel(randFloat() - 0.5f, randFloat() - 0.5f, randFloat() - 0.5f);
|
|
nVel *= VNOISE;
|
|
addVelocity(nVel);
|
|
|
|
addAngularVelocity(RNOISE*(randFloat() - 0.5f),
|
|
RNOISE*(randFloat() - 0.5f),
|
|
RNOISE*(randFloat() - 0.5f));
|
|
}
|
|
position += velocity*deltaTime;
|
|
|
|
pitch += pitchRate;
|
|
yaw += yawRate;
|
|
roll += rollRate;
|
|
|
|
// The spring method
|
|
if (0) {
|
|
// Use a linear spring to attempt to return the hand to the target position
|
|
glm::vec3 springForce = target - position;
|
|
springForce *= LINEAR_SPRING_CONSTANT;
|
|
addVelocity(springForce * deltaTime);
|
|
|
|
// Critically damp the linear spring
|
|
glm::vec3 dampingForce(velocity);
|
|
dampingForce *= LINEAR_DAMPING_COEFFICIENT;
|
|
addVelocity(-dampingForce * deltaTime);
|
|
|
|
// Use angular spring to return hand to target rotation (0,0,0)
|
|
addAngularVelocity(-pitch * ANGULAR_SPRING_CONSTANT * deltaTime,
|
|
-yaw * ANGULAR_SPRING_CONSTANT * deltaTime,
|
|
-roll * ANGULAR_SPRING_CONSTANT * deltaTime);
|
|
|
|
// Critically damp angular spring
|
|
addAngularVelocity(-pitchRate*ANGULAR_DAMPING_COEFFICIENT*deltaTime,
|
|
-yawRate*ANGULAR_DAMPING_COEFFICIENT*deltaTime,
|
|
-rollRate*ANGULAR_DAMPING_COEFFICIENT*deltaTime);
|
|
}
|
|
|
|
// The absolute limits method (no springs)
|
|
if (1) {
|
|
// Limit rotation
|
|
const float YAW_LIMIT = 20;
|
|
const float PITCH_LIMIT = 20;
|
|
|
|
if (yaw > YAW_LIMIT) { yaw = YAW_LIMIT; yawRate = 0.0; }
|
|
if (yaw < -YAW_LIMIT) { yaw = -YAW_LIMIT; yawRate = 0.0; }
|
|
if (pitch > PITCH_LIMIT) { pitch = PITCH_LIMIT; pitchRate = 0.0; }
|
|
if (pitch < -PITCH_LIMIT) { pitch = -PITCH_LIMIT; pitchRate = 0.0; }
|
|
|
|
// Damp Rotation Rates
|
|
yawRate *= 0.99;
|
|
pitchRate *= 0.99;
|
|
rollRate *= 0.99;
|
|
|
|
// Limit position
|
|
const float X_LIMIT = 1.0;
|
|
const float Y_LIMIT = 1.0;
|
|
const float Z_LIMIT = 1.0;
|
|
|
|
if (position.x > DEFAULT_X + X_LIMIT) { position.x = DEFAULT_X + X_LIMIT; velocity.x = 0; }
|
|
if (position.x < DEFAULT_X - X_LIMIT) { position.x = DEFAULT_X - X_LIMIT; velocity.x = 0; }
|
|
if (position.y > DEFAULT_Y + Y_LIMIT) { position.y = DEFAULT_Y + Y_LIMIT; velocity.y = 0; }
|
|
if (position.y < DEFAULT_Y - Y_LIMIT) { position.y = DEFAULT_Y - Y_LIMIT; velocity.y = 0; }
|
|
if (position.z > DEFAULT_Z + Z_LIMIT) { position.z = DEFAULT_Z + Z_LIMIT; velocity.z = 0; }
|
|
if (position.z < DEFAULT_Z - Z_LIMIT) { position.z = DEFAULT_Z - Z_LIMIT; velocity.z = 0; }
|
|
|
|
// Damp Velocity
|
|
velocity *= 0.99;
|
|
|
|
}
|
|
|
|
} |