Plane Plane::Transformed(const Matrix3& transform) const
{
return Plane(Matrix4(transform).Inverse().Transpose() * ToVector4());
}
//------------------------------------------------------------------------------
/// Simple Euler integration
void integrate_actor( void ) {
int qi = 0;
bool failed = true;
SdS sds, sdsi;
unsigned int n = cplist.size();
for( unsigned int i = 2; i < n-1; i++ ) {
sdsi = catmullrom_arclength_map.at( i );
if( actor_ds < sdsi.first ) {
sds = catmullrom_arclength_map.at( i-1 );
qi = i;
failed = false;
break;
}
}
if( failed ) {
std::cout << "failed to find the next control point -> off the spline\n";
return; // failed to find the next control point -> off the spline
}
Eigen::MatrixXd C = CubicSpline::blend( catmullrom_M, cplist, qi );
double qi_ds = sdsi.second;
double qip_s = sds.first;
// where the actor will be in terms of arclength after the step is taken
double s1 = actor_dsdt * sim_dt + (actor_ds - qip_s);
double u = (double)s1 / (double)qi_ds;
Vector3 vi1 = CubicSpline::position( C, u );
// update the actor position
actor_position = vi1;
// advance the actor for next tick
actor_ds = s1 + qip_s;
// advance the sim time
sim_t += sim_dt;
Vector3 v_w = CubicSpline::tangent( C, u );
v_w.normalize();
double wdotup = Vector3::dot( v_w, actor_up );
double wdotforward = Vector3::dot( v_w, actor_forward );
double theta = acos(wdotforward);
if( wdotup < 0.0 ) {
theta = -theta; // pitch down
}
Matrix3 R = Matrix3::rotZ( theta );
actor_up = R * actor_up;
actor_up.normalize();
actor_forward = v_w;
actor_left = Vector3::cross( actor_forward, actor_up );
actor_left.normalize();
actor_transform = Matrix4( actor_forward(0), actor_up(0), actor_left(0), actor_position.x(),
actor_forward(1), actor_up(1), actor_left(1), actor_position.y(),
actor_forward(2), actor_up(2), actor_left(2), actor_position.z(),
0.0, 0.0, 0.0, 1.0 );
}
static Matrix4<T> Identity()
{
return Matrix4();
}
Matrix4 Matrix4::Zero()
{
return Matrix4();
}
#include "header.h"
#include "shader.h"
#include "utilize.h"
#include "model.h"
#pragma comment (lib, "glew32.lib")
#pragma comment (lib, "freeglut.lib")
// Shader attributes
GLint iLocPosition;
GLint iLocColor;
GLint iLocMVP;
Matrix4 M = Matrix4(
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1);
Matrix4 V = Matrix4(
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1);
Matrix4 P = Matrix4(
1, 0, 0, 0,
0, 1, 0, 0,
0, 0, -1, 0,
0, 0, 0, 1);
ModelManager mm = ModelManager("./ColorModels");
int main(int argc, char **argv)
void geode::frustum_draw(Matrix4 c, bool status) {
getBounds(c);
drawWireFrame(c, status);
float m[16], p[16];
glGetFloatv(GL_PROJECTION_MATRIX, p);
glGetFloatv(GL_MODELVIEW_MATRIX, m);
glPushMatrix();
glLoadMatrixf(p);
glMultMatrixf(m);
glGetFloatv(GL_MODELVIEW_MATRIX, m);
glPopMatrix();
Matrix4 m2w = Matrix4(
m[0], m[1], m[2], m[3],
m[4], m[5], m[6], m[7],
m[8], m[9], m[10], m[11],
m[12], m[13], m[14], m[15]);
Vector4 NEARP = Vector4(
m2w.get(2, 0) + m2w.get(3, 0),
m2w.get(2, 1) + m2w.get(3, 1),
m2w.get(2, 2) + m2w.get(3, 2),
m2w.get(2, 3) + m2w.get(3, 3));
Vector4 FARP = Vector4(
-m2w.get(2, 0) + m2w.get(3, 0),
-m2w.get(2, 1) + m2w.get(3, 1),
-m2w.get(2, 2) + m2w.get(3, 2),
-m2w.get(2, 3) + m2w.get(3, 3));
Vector4 BOTTOM = Vector4(
m2w.get(1, 0) + m2w.get(3, 0),
m2w.get(1, 1) + m2w.get(3, 1),
m2w.get(1, 2) + m2w.get(3, 2),
m2w.get(1, 3) + m2w.get(3, 3));
Vector4 TOP = Vector4(
-m2w.get(1, 0) + m2w.get(3, 0),
-m2w.get(1, 1) + m2w.get(3, 1),
-m2w.get(1, 2) + m2w.get(3, 2),
-m2w.get(1, 3) + m2w.get(3, 3));
Vector4 LEFT = Vector4(
m2w.get(0, 0) + m2w.get(3, 0),
m2w.get(0, 1) + m2w.get(3, 1),
m2w.get(0, 2) + m2w.get(3, 2),
m2w.get(0, 3) + m2w.get(3, 3));
Vector4 RIGHT = Vector4(
-m2w.get(0, 0) + m2w.get(3, 0),
-m2w.get(0, 1) + m2w.get(3, 1),
-m2w.get(0, 2) + m2w.get(3, 2),
-m2w.get(0, 3) + m2w.get(3, 3));
NEARP.normalize();
FARP.normalize();
BOTTOM.normalize();
TOP.normalize();
LEFT.normalize();
RIGHT.normalize();
float distance;
int count = 0;
Vector4 bounding = { boundingBox.x, boundingBox.y, boundingBox.z, 1 };
bounding = m2w.multiply(bounding);
bounding.normalize();
Vector4 plane[6] = { NEARP, FARP, BOTTOM, TOP, LEFT, RIGHT };
for (int i = 0; i < 6; i++) {
distance = plane[i][0] * bounding.x + plane[i][1] * bounding.y + plane[i][2] * bounding.z + plane[i][3];
if (distance < -radius) {
break;
}
else {
count++;
}
}
if (count == 6) {
render(c);
}
}
/*
This is the actual matrix data that will be used for the space
conversion matrix. As you can see, it's like an identity matrix,
but swaps the Y and Z axis, and negates the X axis.
*/
static float matrixElements[16] = {
-1, 0, 0, 0,
0, 0, 1, 0,
0, 1, 0, 0,
0, 0, 0, 1
};
/*
static class variables must still be instantiated somewhere!!!
*/
const Matrix4 MD5FileData::conversionMatrix = Matrix4(matrixElements);
MD5FileData::MD5FileData(const std::string &filename) {
std::ifstream f(filename,std::ios::in); //MD5 files are text based, so don't make it an ios::binary ifstream...
if(!f) {
return; //Oh dear!
}
//We have our MD5 file handle!
int numExpectedJoints = 0;
int numExpectedMeshes = 0;
int md5Version = 0;
int numLoadedMeshes = 0;
int numLoadedJoints = 0;
Matrix4 Matrix4::MakePerspective(float fovy, float aspect, float znear, float zfar)
{
D3DXMATRIX mat;
D3DXMatrixPerspectiveFovLH(&mat, fovy, aspect, znear, zfar);
return Matrix4(&mat._11);
}
/**
* @file
* Implementation of 4x4 Matrix class.
*
* @author Mike Smith
*/
#include "Matrix4.h"
#include "vector.h"
#include <math.h>
const Matrix4 Matrix4::Identity = Matrix4(1, 0, 0, 0,
0, 1, 0, 0,
0, 0, 1, 0,
0, 0, 0, 1);
Matrix4::Matrix4() {
toIdent();
}
/** Yes, this is a very ugly constructor. But it's convenient, so there is no
* reason for it to be changed.
*/
Matrix4::Matrix4(float m11, float m12, float m13, float m14,
float m21, float m22, float m23, float m24,
float m31, float m32, float m33, float m34,
float m41, float m42, float m43, float m44) {
_11 = m11;
_12 = m12;
_13 = m13;
_14 = m14;
uut::Matrix4 Matrix4::MakeScaling(const Vector3f& scale)
{
D3DXMATRIX mat;
D3DXMatrixScaling(&mat, scale.x, scale.y, scale.z);
return Matrix4(&mat._11);
}
uut::Matrix4 Matrix4::MakeQuaternion(const Quaternion& quaternion)
{
D3DXMATRIX mat;
D3DXMatrixRotationQuaternion(&mat, (D3DXQUATERNION*)&quaternion);
return Matrix4(mat);
}
Matrix4 Matrix4::MakeRotateZ(float angle)
{
D3DXMATRIX mat;
D3DXMatrixRotationZ(&mat, angle);
return Matrix4(&mat._11);
}
Matrix4 Matrix4::MakeTranslate(const Vector3f& vec)
{
D3DXMATRIX mat;
D3DXMatrixTranslation(&mat, vec.x, vec.y, vec.z);
return Matrix4(&mat._11);
}
Matrix4 Matrix4::MakeIdentity()
{
D3DXMATRIX mat;
D3DXMatrixIdentity(&mat);
return Matrix4(&mat._11);
}
//缩放
Matrix4& Matrix4::changeScale(const Vector3& t) {
*this = *this * Matrix4().createScaleMatrix4(t);
return *this;
}
const Matrix4 Matrix4::operator-(const Matrix4 &rhs) const {
return Matrix4(*this) -= rhs;
}
Matrix4& Matrix4::changeScale(float x, float y, float z) {
*this = *this * Matrix4().createScaleMatrix4(x, y, z);
return *this;
}
const Matrix4 Matrix4::operator*(float scale) const {
return Matrix4(*this) *= scale;
}
geode::geode()
{
myObject = Matrix4();
myObject.identity();
}
void RigidMatrixTransformation3DTest::inverted() {
Matrix4 m = Matrix4::rotationX(Deg(17.0f))*Matrix4::translation({1.0f, -0.3f, 2.3f});
CORRADE_COMPARE(Implementation::Transformation<RigidMatrixTransformation3D>::inverted(m)*m, Matrix4());
}
Matrix4 Deserializer::ReadMatrix4()
{
float data[16];
Read(data, sizeof data);
return Matrix4(data);
}
Matrix4::Matrix4(Vector3 scale, Vector3 Rotation, Vector3 Position) {
*this = Matrix4().createScaleMatrix4(scale) * Matrix4().createRotationMatrix4(Rotation) * Matrix4().createTransitionMatrix4(Position);
}
//-----------------------------------------------------------------------
Matrix4 Matrix4::inverse() const
{
float m00 = m[0][0], m01 = m[0][1], m02 = m[0][2], m03 = m[0][3];
float m10 = m[1][0], m11 = m[1][1], m12 = m[1][2], m13 = m[1][3];
float m20 = m[2][0], m21 = m[2][1], m22 = m[2][2], m23 = m[2][3];
float m30 = m[3][0], m31 = m[3][1], m32 = m[3][2], m33 = m[3][3];
float v0 = m20 * m31 - m21 * m30;
float v1 = m20 * m32 - m22 * m30;
float v2 = m20 * m33 - m23 * m30;
float v3 = m21 * m32 - m22 * m31;
float v4 = m21 * m33 - m23 * m31;
float v5 = m22 * m33 - m23 * m32;
float t00 = + (v5 * m11 - v4 * m12 + v3 * m13);
float t10 = - (v5 * m10 - v2 * m12 + v1 * m13);
float t20 = + (v4 * m10 - v2 * m11 + v0 * m13);
float t30 = - (v3 * m10 - v1 * m11 + v0 * m12);
float invDet = 1 / (t00 * m00 + t10 * m01 + t20 * m02 + t30 * m03);
float d00 = t00 * invDet;
float d10 = t10 * invDet;
float d20 = t20 * invDet;
float d30 = t30 * invDet;
float d01 = - (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
float d11 = + (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
float d21 = - (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
float d31 = + (v3 * m00 - v1 * m01 + v0 * m02) * invDet;
v0 = m10 * m31 - m11 * m30;
v1 = m10 * m32 - m12 * m30;
v2 = m10 * m33 - m13 * m30;
v3 = m11 * m32 - m12 * m31;
v4 = m11 * m33 - m13 * m31;
v5 = m12 * m33 - m13 * m32;
float d02 = + (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
float d12 = - (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
float d22 = + (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
float d32 = - (v3 * m00 - v1 * m01 + v0 * m02) * invDet;
v0 = m21 * m10 - m20 * m11;
v1 = m22 * m10 - m20 * m12;
v2 = m23 * m10 - m20 * m13;
v3 = m22 * m11 - m21 * m12;
v4 = m23 * m11 - m21 * m13;
v5 = m23 * m12 - m22 * m13;
float d03 = - (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
float d13 = + (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
float d23 = - (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
float d33 = + (v3 * m00 - v1 * m01 + v0 * m02) * invDet;
return Matrix4(
d00, d01, d02, d03,
d10, d11, d12, d13,
d20, d21, d22, d23,
d30, d31, d32, d33);
}
//给Vector3创造旋转矩阵
Matrix4& Matrix4::createRotationMatrix4(const Vector3& b) {
*this = Matrix4('x', b.x) * Matrix4('y', b.y) * Matrix4('z', b.z);
return *this;
}
void Branch::Update(float msec)
{
mesh->Update(msec);
SceneNode::Update(msec);
if (mesh->GetDrawIndices() == (branchSize * 24) && numLeafs < maxLeafs)
{
SceneNode* leaf = new SceneNode(sphere);
float leafpos = (float)((mesh->GetDrawIndices() / 24) / 5);
leaf->SetTransform(Matrix4::Translation(Vector3(0, leafpos * maxScale.y, 0)));
leaf->SetModelScale(Vector3(20,20,20));
AddChild(leaf);
leaf->Update(msec);
///////
PhysicsNode*p = new PhysicsNode();
float elements[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1};
Matrix4 mat = Matrix4(elements);
p->SetInverseInertia(mat);
p->SetUseGravity(false);
p->SetInverseMass(0.0f);
p->SetPosition(leaf->GetWorldTransform().GetPositionVector());
p->SetCollisionVolume(new CollisionLeaf(20));
Vector3 startPos = GetWorldTransform().GetPositionVector();
Vector3 endPos = leaf->GetWorldTransform().GetPositionVector();
PhysicsSystem::GetPhysicsSystem().AddNode(p);
/////////
Light l = Light();
l.SetPosition(leaf->GetWorldTransform().GetPositionVector());
l.SetColour(Vector4 (0, 1, 0, 1.0f));
l.SetRadius(150);
LightStorage::GetInstance()->addLight(l);
numLeafs++;
}
if (modelScale.x < maxScale.x)
{
modelScale.x += 0.000001;
modelScale.z += 0.000001;
}
if (!hasPhysics && GetWorldTransform().GetPositionVector().Length() != 0)
{
SceneNode* leaf = new SceneNode(sphere);
float leafpos = (float)((mesh->getNumInd() / 24) / 5);
leaf->SetTransform(Matrix4::Translation(Vector3(0, leafpos * maxScale.y, 0)));
AddChild(leaf);
leaf->Update(msec);
///////
PhysicsNode*p = new PhysicsNode();
float elements[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1};
Matrix4 mat = Matrix4(elements);
p->SetInverseInertia(mat);
p->SetUseGravity(false);
p->SetInverseMass(0.0f);
p->SetPosition(leaf->GetWorldTransform().GetPositionVector());
Vector3 startPos = GetWorldTransform().GetPositionVector();
Vector3 endPos = leaf->GetWorldTransform().GetPositionVector();
endBranch = endPos;
p->SetCollisionVolume(new CollisionCylinder(1, startPos, endPos));
myNode = p;
PhysicsSystem::GetPhysicsSystem().AddNode(p);
/////////
hasPhysics = true;
RemoveChild(leaf);
}
if (myNode)
{
float v = mesh->GetDrawIndices();
float z = v / (branchSize * 24);
Vector3 posDif = endBranch - GetWorldTransform().GetPositionVector();
Vector3 endPos = GetWorldTransform().GetPositionVector() + (posDif * z);
myNode->SetCollisionVolume(new CollisionCylinder(2, GetWorldTransform().GetPositionVector(), endPos));
}
}
//给出3个轴的旋转角，创建旋转矩阵
Matrix4& Matrix4::createRotationMatrix4(float x, float y, float z) {
*this = Matrix4('x', x)*Matrix4('y', y)*Matrix4('z', z);
return *this;
}
Matrix4 CoordinateFrame::toMatrix4() const {
return Matrix4(*this);
}
//旋转
Matrix4& Matrix4::changeRotationSingleAxis(char axis, float degree) {
*this = *this * Matrix4(axis, degree);
return *this;
}
//-----------------------------------------------------------------------
Matrix4 Matrix4::inverse() const
{
Real m00 = m[0][0], m01 = m[0][1], m02 = m[0][2], m03 = m[0][3];
Real m10 = m[1][0], m11 = m[1][1], m12 = m[1][2], m13 = m[1][3];
Real m20 = m[2][0], m21 = m[2][1], m22 = m[2][2], m23 = m[2][3];
Real m30 = m[3][0], m31 = m[3][1], m32 = m[3][2], m33 = m[3][3];
Real v0 = m20 * m31 - m21 * m30;
Real v1 = m20 * m32 - m22 * m30;
Real v2 = m20 * m33 - m23 * m30;
Real v3 = m21 * m32 - m22 * m31;
Real v4 = m21 * m33 - m23 * m31;
Real v5 = m22 * m33 - m23 * m32;
Real t00 = + (v5 * m11 - v4 * m12 + v3 * m13);
Real t10 = - (v5 * m10 - v2 * m12 + v1 * m13);
Real t20 = + (v4 * m10 - v2 * m11 + v0 * m13);
Real t30 = - (v3 * m10 - v1 * m11 + v0 * m12);
Real invDet = 1 / (t00 * m00 + t10 * m01 + t20 * m02 + t30 * m03);
Real d00 = t00 * invDet;
Real d10 = t10 * invDet;
Real d20 = t20 * invDet;
Real d30 = t30 * invDet;
Real d01 = - (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
Real d11 = + (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
Real d21 = - (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
Real d31 = + (v3 * m00 - v1 * m01 + v0 * m02) * invDet;
v0 = m10 * m31 - m11 * m30;
v1 = m10 * m32 - m12 * m30;
v2 = m10 * m33 - m13 * m30;
v3 = m11 * m32 - m12 * m31;
v4 = m11 * m33 - m13 * m31;
v5 = m12 * m33 - m13 * m32;
Real d02 = + (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
Real d12 = - (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
Real d22 = + (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
Real d32 = - (v3 * m00 - v1 * m01 + v0 * m02) * invDet;
v0 = m21 * m10 - m20 * m11;
v1 = m22 * m10 - m20 * m12;
v2 = m23 * m10 - m20 * m13;
v3 = m22 * m11 - m21 * m12;
v4 = m23 * m11 - m21 * m13;
v5 = m23 * m12 - m22 * m13;
Real d03 = - (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
Real d13 = + (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
Real d23 = - (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
Real d33 = + (v3 * m00 - v1 * m01 + v0 * m02) * invDet;
return Matrix4(
d00, d01, d02, d03,
d10, d11, d12, d13,
d20, d21, d22, d23,
d30, d31, d32, d33);
}
void Plane::Transform(const Matrix3& transform)
{
Define(Matrix4(transform).Inverse().Transpose() * ToVector4());
}
