1 /*************************************************************************
2 * *
3 * Vega FEM Simulation Library Version 3.1 *
4 * *
5 * "sceneObject" library , Copyright (C) 2007 CMU, 2009 MIT, 2016 USC *
6 * All rights reserved. *
7 * *
8 * Code authors: Jernej Barbic, Daniel Schroeder *
9 * http://www.jernejbarbic.com/code *
10 * *
11 * Research: Jernej Barbic, Fun Shing Sin, Daniel Schroeder, *
12 * Doug L. James, Jovan Popovic *
13 * *
14 * Funding: National Science Foundation, Link Foundation, *
15 * Singapore-MIT GAMBIT Game Lab, *
16 * Zumberge Research and Innovation Fund at USC *
17 * *
18 * This library is free software; you can redistribute it and/or *
19 * modify it under the terms of the BSD-style license that is *
20 * included with this library in the file LICENSE.txt *
21 * *
22 * This library is distributed in the hope that it will be useful, *
23 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
24 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the file *
25 * LICENSE.TXT for more details. *
26 * *
27 *************************************************************************/
28
29 #include "sceneObjectReduced6DOF.h"
30
SceneObjectReduced6DOF(const char * filenameOBJ,ModalMatrix * modalMatrix)31 SceneObjectReduced6DOF::SceneObjectReduced6DOF(const char * filenameOBJ, ModalMatrix * modalMatrix): SceneObjectWithRestPosition(filenameOBJ), SceneObjectReduced(filenameOBJ, modalMatrix), SceneObject6DOF(filenameOBJ), qvel(NULL)
32 {
33 }
34
SceneObjectReduced6DOF(ObjMesh * objMesh,ModalMatrix * modalMatrix,bool deepCopy)35 SceneObjectReduced6DOF::SceneObjectReduced6DOF(ObjMesh * objMesh, ModalMatrix * modalMatrix, bool deepCopy): SceneObjectWithRestPosition(objMesh, deepCopy), SceneObjectReduced(objMesh, modalMatrix, deepCopy), SceneObject6DOF(objMesh, deepCopy), qvel(NULL)
36 {
37 }
38
~SceneObjectReduced6DOF()39 SceneObjectReduced6DOF::~SceneObjectReduced6DOF()
40 {
41 free(qvel);
42 }
43
GetSingleVertexPosition(int vertex,double * x,double * y,double * z)44 void SceneObjectReduced6DOF::GetSingleVertexPosition(int vertex, double * x, double * y, double * z)
45 {
46 double x0 = restPosition[3*vertex+0];
47 double y0 = restPosition[3*vertex+1];
48 double z0 = restPosition[3*vertex+2];
49
50 modalMatrix->AddAssembleSingleVertex(vertex,q,&x0,&y0,&z0);
51
52 // transform x0, y0, z0 via centerOfMass,R
53 // x = centerOfMass + R * x0
54 *x = R[0] * x0 + R[1] * y0 + R[2] * z0 + centerOfMass[0];
55 *y = R[3] * x0 + R[4] * y0 + R[5] * z0 + centerOfMass[1];
56 *z = R[6] * x0 + R[7] * y0 + R[8] * z0 + centerOfMass[2];
57 }
58
GetSingleVertexVelocity(int vertex,double * objectVel,double * objectAngVel,double * velx,double * vely,double * velz)59 void SceneObjectReduced6DOF::GetSingleVertexVelocity(int vertex, double * objectVel, double * objectAngVel, double * velx, double * vely, double * velz)
60 {
61 SceneObject6DOF::GetSingleVertexVelocity(vertex, objectVel, objectAngVel, velx, vely, velz);
62
63 double localDefoVel[3];
64 modalMatrix->AssembleSingleVertex(vertex,qvel,&localDefoVel[0], &localDefoVel[1], &localDefoVel[2]);
65 *velx += R[0] * localDefoVel[0] + R[1] * localDefoVel[1] + R[2] * localDefoVel[2];
66 *vely += R[3] * localDefoVel[0] + R[4] * localDefoVel[1] + R[5] * localDefoVel[2];
67 *velz += R[6] * localDefoVel[0] + R[7] * localDefoVel[1] + R[8] * localDefoVel[2];
68 }
69
Setqvel(double * qvel_)70 void SceneObjectReduced6DOF::Setqvel(double * qvel_)
71 {
72 int r = modalMatrix->Getr();
73 if (qvel == NULL)
74 qvel = (double*) malloc (sizeof(double) * r);
75 memcpy(qvel, qvel_, sizeof(double) * r);
76 }
77
78
79